#include "PointCloudGLWidget.h"
#include <QDebug>
#include <QtMath>
#include <QDialog>
#include <QDialogButtonBox>
#include <QDoubleSpinBox>
#include <QFormLayout>
#include <QGestureEvent>
#include <QHBoxLayout>
#include <QOpenGLContext>
#include <QPainter>
#include <QPinchGesture>
#include <QPushButton>
#include <QResizeEvent>
#include <QToolButton>
#include <QVBoxLayout>
#include <algorithm>
#include <cfloat>
#include <cmath>
#include <map>
#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

namespace
{
// 将角度归一化到 [-180, 180]，避免往同一方向旋转时无限累加
float NormalizeAngleDegrees(float angle)
{
    float wrapped = std::fmod(angle, 360.0f);
    if (wrapped > 180.0f) {
        wrapped -= 360.0f;
    } else if (wrapped < -180.0f) {
        wrapped += 360.0f;
    }
    return wrapped;
}
}

PointCloudGLWidget::PointCloudGLWidget(QWidget* parent)
    : QOpenGLWidget(parent)
    , m_shader(nullptr)
    , m_shaderReady(false)
    , m_mvpLocation(0)
    , m_positionLocation(0)
    , m_colorLocation(0)
    , m_pointSizeLocation(0)
    , m_alphaLocation(0)
    , m_useVertexColorLocation(0)
    , m_uniformColorLocation(0)
    , m_distance(100.0f)
    , m_pinchStartDistance(100.0f)
    , m_rotationX(0.0f)
    , m_rotationY(0.0f)
    , m_rotationZ(0.0f)
    , m_rotation(QQuaternion())  // 初始化为单位四元数
    , 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)
    , m_viewToolbar(nullptr)
    , m_viewAngleButton(nullptr)
    , m_zoomInButton(nullptr)
    , m_zoomOutButton(nullptr)
{
    setFocusPolicy(Qt::StrongFocus);
    setMouseTracking(true);
    setAttribute(Qt::WA_AcceptTouchEvents, true);
    grabGesture(Qt::PinchGesture);

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

    createOverlayControls();
    connect(this, &PointCloudGLWidget::viewAnglesChanged,
            this, [this](float, float, float) { updateViewAngleDisplay(); });
}

bool PointCloudGLWidget::event(QEvent* event)
{
    if (event->type() == QEvent::Gesture) {
        auto* gestureEvent = static_cast<QGestureEvent*>(event);
        if (auto* pinch = static_cast<QPinchGesture*>(
                gestureEvent->gesture(Qt::PinchGesture))) {
            const bool handled = handlePinchGesture(pinch);
            if (handled) {
                gestureEvent->accept(pinch);
                return true;
            }
        }
    }
    return QOpenGLWidget::event(event);
}

bool PointCloudGLWidget::handlePinchGesture(QPinchGesture* gesture)
{
    if (!gesture) {
        return false;
    }

    if (gesture->state() == Qt::GestureStarted) {
        m_pinchStartDistance = m_distance;
    }

    const qreal scale = gesture->totalScaleFactor();
    if (scale <= 0.0 || !std::isfinite(static_cast<double>(scale))) {
        return false;
    }

    m_distance = qBound(minimumCameraDistance(),
                        m_pinchStartDistance / static_cast<float>(scale),
                        1.0e7f);
    update();
    return true;
}

PointCloudGLWidget::~PointCloudGLWidget()
{
    // 在 GL 上下文中释放所有 VBO
    makeCurrent();
    for (auto& cloudData : m_pointClouds) {
        releaseVBO(cloudData);
    }
    if (m_shader) {
        delete m_shader;
        m_shader = nullptr;
    }
    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::createOverlayControls()
{
    const int buttonSize = 32;
    const QString toolButtonStyle =
        "QToolButton {"
        "  background-color: rgba(20, 36, 58, 200);"
        "  color: #d8e8f4;"
        "  border: 1px solid #2b4f77;"
        "  border-radius: 4px;"
        "  font-size: 13px;"
        "}"
        "QToolButton:hover { background-color: #204a72; }"
        "QToolButton:pressed { background-color: #10243a; }";

    // ── 左侧视图方向工具栏（纵向排布） ──
    struct ViewPreset {
        const char* label;
        const char* tooltip;
        float rx, ry, rz;
    };
    // 坐标系定义：X向右，Y向下，Z朝后（与 CloudView 一致）
    const ViewPreset presets[] = {
        {"前", "正视 (XY面)",  180.0f,   0.0f,   0.0f},
        {"后", "后视",          180.0f, 180.0f,   0.0f},
        {"左", "左视 (YZ面)",   180.0f,  90.0f,   0.0f},
        {"右", "右视",          180.0f, -90.0f,   0.0f},
        {"上", "俯视 (XZ面)",    90.0f,   0.0f,   0.0f},
        {"下", "仰视",          -90.0f,   0.0f,   0.0f},
        {"机", "机械臂",         -90.0f,  90.0f,   0.0f},
    };

    m_viewToolbar = new QWidget(this);
    m_viewToolbar->setStyleSheet("background: transparent;");
    auto* toolbarLayout = new QVBoxLayout(m_viewToolbar);
    toolbarLayout->setContentsMargins(4, 4, 4, 4);
    toolbarLayout->setSpacing(4);
    // 工具栏尺寸跟随内容，便于垂直居中定位
    toolbarLayout->setSizeConstraint(QLayout::SetFixedSize);

    for (const ViewPreset& preset : presets) {
        auto* button = new QToolButton(m_viewToolbar);
        button->setText(QString::fromUtf8(preset.label));
        button->setToolTip(QString::fromUtf8(preset.tooltip));
        button->setFixedSize(buttonSize, buttonSize);
        button->setStyleSheet(toolButtonStyle);
        const float rx = preset.rx;
        const float ry = preset.ry;
        const float rz = preset.rz;
        connect(button, &QToolButton::clicked, this, [this, rx, ry, rz]() {
            setViewAngles(rx, ry, rz);
        });
        m_viewButtons.append(button);
        toolbarLayout->addWidget(button);
    }
    toolbarLayout->addStretch(1);
    m_viewToolbar->setFixedWidth(buttonSize + 12);
    m_viewToolbar->show();

    // ── 左下角旋转角度显示按钮（点击弹窗输入角度） ──
    m_viewAngleButton = new QPushButton(this);
    m_viewAngleButton->setStyleSheet(
        "QPushButton {"
        "  background-color: rgba(20, 36, 58, 200);"
        "  color: #d8e8f4;"
        "  border: 1px solid #2b4f77;"
        "  border-radius: 4px;"
        "  padding: 4px 8px;"
        "  text-align: left;"
        "}"
        "QPushButton:hover { background-color: #204a72; }");
    connect(m_viewAngleButton, &QPushButton::clicked,
            this, &PointCloudGLWidget::showViewAngleDialog);
    // 显式指定字体，保证 fontMetrics 度量与实际渲染一致
    QFont angleFont = m_viewAngleButton->font();
    angleFont.setPixelSize(12);
    m_viewAngleButton->setFont(angleFont);
    // 竖向三行显示 RX/RY/RZ：宽度按最长一行算，高度按三行算
    const QString longestAngleLine = QStringLiteral("RX:-360.0");
    const int angleTextWidth =
        m_viewAngleButton->fontMetrics().horizontalAdvance(longestAngleLine);
    const int angleLineHeight = m_viewAngleButton->fontMetrics().lineSpacing();
    m_viewAngleButton->setFixedSize(angleTextWidth + 24, angleLineHeight * 3 + 16);
    m_viewAngleButton->show();

    // ── 右侧悬浮缩放按钮（垂直居中，与 CloudView 位置一致） ──
    const QString zoomButtonStyle =
        "QToolButton {"
        "  background-color: rgba(20, 36, 58, 200);"
        "  color: #e8f2fb;"
        "  border: 1px solid #2b4f77;"
        "  border-radius: 4px;"
        "  font-size: 26px;"
        "  font-weight: bold;"
        "}"
        "QToolButton:hover { background-color: #204a72; }"
        "QToolButton:pressed { background-color: #10243a; }";
    const int zoomButtonSize = 28;
    m_zoomInButton = new QToolButton(this);
    m_zoomInButton->setText("+");
    m_zoomInButton->setFixedSize(zoomButtonSize, zoomButtonSize);
    m_zoomInButton->setStyleSheet(zoomButtonStyle);
    m_zoomInButton->setAutoRepeat(true);
    m_zoomInButton->setAutoRepeatDelay(300);
    m_zoomInButton->setAutoRepeatInterval(50);
    connect(m_zoomInButton, &QToolButton::clicked, this, &PointCloudGLWidget::zoomIn);
    m_zoomInButton->show();

    m_zoomOutButton = new QToolButton(this);
    m_zoomOutButton->setText("-");
    m_zoomOutButton->setFixedSize(zoomButtonSize, zoomButtonSize);
    m_zoomOutButton->setStyleSheet(zoomButtonStyle);
    m_zoomOutButton->setAutoRepeat(true);
    m_zoomOutButton->setAutoRepeatDelay(300);
    m_zoomOutButton->setAutoRepeatInterval(50);
    connect(m_zoomOutButton, &QToolButton::clicked, this, &PointCloudGLWidget::zoomOut);
    m_zoomOutButton->show();

    updateViewAngleDisplay();
    repositionOverlayControls();
}

void PointCloudGLWidget::repositionOverlayControls()
{
    const int widgetWidth = width();
    const int widgetHeight = height();
    if (widgetWidth <= 0 || widgetHeight <= 0) {
        return;
    }

    // 左侧视图工具栏：垂直居中
    if (m_viewToolbar) {
        m_viewToolbar->move(8, qMax(0, (widgetHeight - m_viewToolbar->height()) / 2));
    }

    // 左下角旋转角度按钮
    if (m_viewAngleButton) {
        m_viewAngleButton->adjustSize();
        const int margin = 8;
        m_viewAngleButton->move(margin,
                                widgetHeight - m_viewAngleButton->height() - margin);
    }

    // 右侧缩放按钮：垂直居中
    if (m_zoomInButton && m_zoomOutButton) {
        const int buttonWidth = m_zoomInButton->width();
        const int buttonHeight = m_zoomInButton->height();
        const int margin = 8;
        const int gap = 4;
        const int x = widgetWidth - buttonWidth - margin;
        const int y = qMax(0, (widgetHeight - buttonHeight * 2 - gap) / 2);
        m_zoomInButton->move(x, y);
        m_zoomOutButton->move(x, y + buttonHeight + gap);
    }
}

void PointCloudGLWidget::resizeEvent(QResizeEvent* event)
{
    QOpenGLWidget::resizeEvent(event);
    repositionOverlayControls();
}

void PointCloudGLWidget::updateViewAngleDisplay()
{
    if (!m_viewAngleButton) {
        return;
    }
    m_viewAngleButton->setText(QStringLiteral("RX:%1\nRY:%2\nRZ:%3")
                                   .arg(m_rotationX, 0, 'f', 1)
                                   .arg(m_rotationY, 0, 'f', 1)
                                   .arg(m_rotationZ, 0, 'f', 1));
}

void PointCloudGLWidget::showViewAngleDialog()
{
    QDialog dialog(this);
    dialog.setWindowTitle(QStringLiteral("设置视图角度"));
    dialog.setMinimumWidth(240);

    auto* form = new QFormLayout(&dialog);
    auto* spinX = new QDoubleSpinBox(&dialog);
    auto* spinY = new QDoubleSpinBox(&dialog);
    auto* spinZ = new QDoubleSpinBox(&dialog);
    for (QDoubleSpinBox* spin : {spinX, spinY, spinZ}) {
        spin->setRange(-180.0, 180.0);
        spin->setDecimals(1);
        spin->setSingleStep(1.0);
        spin->setSuffix(QStringLiteral(" °"));
    }
    spinX->setValue(m_rotationX);
    spinY->setValue(m_rotationY);
    spinZ->setValue(m_rotationZ);
    form->addRow(QStringLiteral("RX"), spinX);
    form->addRow(QStringLiteral("RY"), spinY);
    form->addRow(QStringLiteral("RZ"), spinZ);

    auto* buttons = new QDialogButtonBox(
        QDialogButtonBox::Ok | QDialogButtonBox::Cancel, &dialog);
    buttons->button(QDialogButtonBox::Ok)->setText(QStringLiteral("确认"));
    buttons->button(QDialogButtonBox::Cancel)->setText(QStringLiteral("取消"));
    connect(buttons, &QDialogButtonBox::accepted, &dialog, &QDialog::accept);
    connect(buttons, &QDialogButtonBox::rejected, &dialog, &QDialog::reject);
    form->addRow(buttons);

    if (dialog.exec() == QDialog::Accepted) {
        setViewAngles(static_cast<float>(spinX->value()),
                      static_cast<float>(spinY->value()),
                      static_cast<float>(spinZ->value()));
    }
}

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);
    // 桌面 GL（GL 3.2+）下，顶点着色器输出的 gl_PointSize 需显式开启才生效；
    // GLES2 总是使用 shader 点大小，无需此开关。
    if (QOpenGLContext::currentContext() &&
        !QOpenGLContext::currentContext()->isOpenGLES()) {
#ifdef GL_PROGRAM_POINT_SIZE
        glEnable(GL_PROGRAM_POINT_SIZE);
#endif
    }
    if (!m_dynamicVbo.isCreated()) {
        m_dynamicVbo.create();
    }
    m_shaderReady = false;
    ensureShader();
}

bool PointCloudGLWidget::ensureShader()
{
    if (m_shaderReady) {
        return m_shader != nullptr;
    }
    m_shaderReady = true;

    if (!m_shader) {
        auto* program = new QOpenGLShaderProgram();
        program->bindAttributeLocation("aPosition", 0);
        program->bindAttributeLocation("aColor", 1);

        if (!program->addShaderFromSourceCode(
                QOpenGLShader::Vertex,
                // GLSL ES 1.00 的 vertex shader 有默认 float 精度（highp），
                // 但显式声明更稳妥，兼容性更好。
                "precision highp float;\n"
                "attribute vec3 aPosition;\n"
                "attribute vec3 aColor;\n"
                "uniform mat4 uMvp;\n"
                "uniform float uPointSize;\n"
                "uniform float uAlpha;\n"
                "uniform float uUseVertexColor;\n"
                "uniform vec3 uColor;\n"
                "varying vec3 vColor;\n"
                "varying float vAlpha;\n"
                "void main() {\n"
                "    gl_Position = uMvp * vec4(aPosition, 1.0);\n"
                "    gl_PointSize = uPointSize;\n"
                "    vColor = (uUseVertexColor > 0.5) ? aColor : uColor;\n"
                "    vAlpha = uAlpha;\n"
                "}\n") ||
            !program->addShaderFromSourceCode(
                QOpenGLShader::Fragment,
                // GLSL ES 1.00 的 fragment shader 没有默认 float 精度，
                // 必须显式声明，否则 Mali 等 GLES 驱动编译失败（S0032）。
                "precision highp float;\n"
                "varying vec3 vColor;\n"
                "varying float vAlpha;\n"
                "void main() {\n"
                "    gl_FragColor = vec4(vColor, vAlpha);\n"
                "}\n")) {
            LOG_ERROR("[CloudView] Shader compile failed: %s\n",
                      program->log().isEmpty()
                          ? "unknown error"
                          : qPrintable(program->log()));
            delete program;
            return false;
        }

        if (!program->link()) {
            LOG_ERROR("[CloudView] Shader link failed: %s\n",
                      program->log().isEmpty()
                          ? "unknown error"
                          : qPrintable(program->log()));
            delete program;
            return false;
        }

        m_shader = program;
        m_positionLocation = m_shader->attributeLocation("aPosition");
        m_colorLocation = m_shader->attributeLocation("aColor");
        m_mvpLocation = m_shader->uniformLocation("uMvp");
        m_pointSizeLocation = m_shader->uniformLocation("uPointSize");
        m_alphaLocation = m_shader->uniformLocation("uAlpha");
        m_useVertexColorLocation = m_shader->uniformLocation("uUseVertexColor");
        m_uniformColorLocation = m_shader->uniformLocation("uColor");
    }
    return m_shader != nullptr;
}

QVector3D PointCloudGLWidget::colorForIndex(int colorIndex) const
{
    static const QVector3D colorTable[COLOR_COUNT] = {
        QVector3D(0.75f, 0.75f, 0.75f),  // 浅灰
        QVector3D(0.3f, 1.0f, 0.3f),     // 浅绿
        QVector3D(0.3f, 0.3f, 1.0f),     // 浅蓝
        QVector3D(1.0f, 1.0f, 0.3f),     // 黄
        QVector3D(0.3f, 1.0f, 1.0f),     // 青
        QVector3D(1.0f, 0.3f, 1.0f),     // 品红
        QVector3D(1.0f, 1.0f, 1.0f)      // 白
    };
    return colorTable[colorIndex % COLOR_COUNT];
}

void PointCloudGLWidget::drawCloud(PointCloudData& data, float pointSize)
{
    if (data.vertices.empty() || !m_shader) {
        return;
    }
    m_shader->bind();
    glUniformMatrix4fv(m_mvpLocation, 1, GL_FALSE, m_mvp.constData());
    glUniform1f(m_pointSizeLocation, pointSize);
    glUniform1f(m_alphaLocation, 1.0f);
    // 兜底：桌面 GL 未开启 GL_PROGRAM_POINT_SIZE 时，点大小取自 GL_POINT_SIZE 状态
    glPointSize(pointSize);

    data.vertexBuffer.bind();
    glEnableVertexAttribArray(m_positionLocation);
    glVertexAttribPointer(m_positionLocation, 3, GL_FLOAT, GL_FALSE, 0, nullptr);

    const bool useVertexColor = data.colorBuffer.isCreated() && !data.colors.empty();
    if (useVertexColor) {
        data.colorBuffer.bind();
        glEnableVertexAttribArray(m_colorLocation);
        glVertexAttribPointer(m_colorLocation, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
        glUniform1f(m_useVertexColorLocation, 1.0f);
    } else {
        glDisableVertexAttribArray(m_colorLocation);
        const QVector3D color = colorForIndex(data.colorIndex);
        glUniform3f(m_uniformColorLocation, color.x(), color.y(), color.z());
        glUniform1f(m_useVertexColorLocation, 0.0f);
    }

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

    glDisableVertexAttribArray(m_positionLocation);
    if (useVertexColor) {
        glDisableVertexAttribArray(m_colorLocation);
    }
    data.vertexBuffer.release();
    data.colorBuffer.release();
}

void PointCloudGLWidget::drawPrimitives(const QMatrix4x4& mvp,
                                        const float* positions,
                                        int vertexCount,
                                        GLenum mode,
                                        const float* colors,
                                        const QVector3D& uniformColor,
                                        float pointSize,
                                        float alpha,
                                        bool depthTest)
{
    if (!positions || vertexCount <= 0 || !m_shader) {
        return;
    }
    m_shader->bind();
    glUniformMatrix4fv(m_mvpLocation, 1, GL_FALSE, mvp.constData());
    glUniform1f(m_pointSizeLocation, pointSize);
    glUniform1f(m_alphaLocation, alpha);
    // 兜底：桌面 GL 未开启 GL_PROGRAM_POINT_SIZE 时，点大小取自 GL_POINT_SIZE 状态
    glPointSize(pointSize);
    if (depthTest) {
        glEnable(GL_DEPTH_TEST);
    } else {
        glDisable(GL_DEPTH_TEST);
    }

    const GLsizeiptr positionBytes =
        static_cast<GLsizeiptr>(vertexCount) * 3 * sizeof(float);
    const GLsizeiptr colorBytes =
        colors ? static_cast<GLsizeiptr>(vertexCount) * 3 * sizeof(float) : 0;

    m_dynamicVbo.bind();
    m_dynamicVbo.allocate(positionBytes + colorBytes);
    m_dynamicVbo.write(0, positions, positionBytes);
    if (colors) {
        m_dynamicVbo.write(positionBytes, colors, colorBytes);
    }

    glEnableVertexAttribArray(m_positionLocation);
    glVertexAttribPointer(m_positionLocation, 3, GL_FLOAT, GL_FALSE, 0, nullptr);

    if (colors) {
        glEnableVertexAttribArray(m_colorLocation);
        glVertexAttribPointer(m_colorLocation, 3, GL_FLOAT, GL_FALSE, 0,
                              reinterpret_cast<void*>(positionBytes));
        glUniform1f(m_useVertexColorLocation, 1.0f);
    } else {
        glDisableVertexAttribArray(m_colorLocation);
        glUniform3f(m_uniformColorLocation,
                    uniformColor.x(), uniformColor.y(), uniformColor.z());
        glUniform1f(m_useVertexColorLocation, 0.0f);
    }

    glDrawArrays(mode, 0, vertexCount);

    m_dynamicVbo.release();
    glDisableVertexAttribArray(m_positionLocation);
    if (colors) {
        glDisableVertexAttribArray(m_colorLocation);
    }
    glEnable(GL_DEPTH_TEST);
}

void PointCloudGLWidget::resetRenderState()
{
    glDisableVertexAttribArray(m_positionLocation);
    glDisableVertexAttribArray(m_colorLocation);
    m_dynamicVbo.release();
    if (m_shader) {
        m_shader->release();
    }
    glEnable(GL_DEPTH_TEST);
}

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

void PointCloudGLWidget::paintGL()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    if (!ensureShader()) {
        return;
    }

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

    // 使用四元数旋转（物体坐标系旋转）
    m_view.rotate(m_rotation);

    m_view.translate(-m_center);

    // 调试输出：每100帧输出一次当前旋转角度
    static int frameCount = 0;
    if (++frameCount % 100 == 0) {
        // 从四元数转换为欧拉角用于显示
        QVector3D euler = m_rotation.toEulerAngles();
        LOG_INFO("[CloudView] Current rotation: pitch=%.1f, yaw=%.1f, roll=%.1f\n",
                 euler.x(), euler.y(), euler.z());
    }

    QMatrix4x4 modelView = m_view * m_model;
    updateProjection(modelView);
    m_mvp = m_projection * modelView;

    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);
        }

        drawCloud(cloudData, m_pointSize);

        // 绘制自定义大小的点（RGBA 中 A > 1 的点）
        if (cloudData.hasCustomPointSizes) {
            glDepthFunc(GL_LEQUAL);  // 允许在相同深度覆盖绘制
            const bool cloudHasColor = cloudData.colorBuffer.isCreated() &&
                                       !cloudData.colors.empty();
            for (const auto& group : cloudData.customPointSizeGroups) {
                QVector<float> positions;
                positions.reserve(static_cast<int>(group.indices.size()) * 3);
                QVector<float> colors;
                if (cloudHasColor) {
                    colors.reserve(static_cast<int>(group.indices.size()) * 3);
                }
                for (size_t idx : group.indices) {
                    const size_t vi = idx * 3;
                    if (vi + 2 < cloudData.vertices.size()) {
                        positions.append(cloudData.vertices[vi]);
                        positions.append(cloudData.vertices[vi + 1]);
                        positions.append(cloudData.vertices[vi + 2]);
                        if (cloudHasColor && vi + 2 < cloudData.colors.size()) {
                            colors.append(cloudData.colors[vi]);
                            colors.append(cloudData.colors[vi + 1]);
                            colors.append(cloudData.colors[vi + 2]);
                        }
                    }
                }
                if (!positions.isEmpty()) {
                    drawPrimitives(m_mvp,
                                   positions.constData(),
                                   positions.size() / 3,
                                   GL_POINTS,
                                   cloudHasColor ? colors.constData() : nullptr,
                                   colorForIndex(cloudData.colorIndex),
                                   group.pointSize,
                                   1.0f,
                                   true);
                }
            }
            glDepthFunc(GL_LESS);  // 恢复默认深度测试
        }
    }

    drawSelectedPoints();
    drawMeasurementLine();
    drawSelectedLine();
    drawLineSegments();
    drawBasicShapeSurfaces();
    drawBasicShapeSegments();
    drawBasicShapePoints();
    drawPosePoints();

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

    // 清理 GL 状态，避免影响 QPainter 标注
    resetRenderState();

    // 使用 QPainter 绘制坐标轴 XYZ 标注
    drawAxisLabels();
}

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;
    int prevLineIdx = -1;
    int ptInLineCounter = 0;
    for (size_t i = 0; i < cloud.points.size(); ++i) {
        // 跟踪线内索引（对所有点递增，包括被过滤的零点）
        int lineIdx = -1;
        if (data.hasLineInfo && i < cloud.lineIndices.size()) {
            lineIdx = cloud.lineIndices[i];
        }
        if (lineIdx != prevLineIdx) {
            ptInLineCounter = 0;
            prevLineIdx = lineIdx;
        }

        const auto& pt = cloud.points[i];
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            ptInLineCounter++;
            continue;
        }
        // 显示时过滤 (0,0,0) 点
        if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
            ptInLineCounter++;
            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));

        // 保存线内索引（点在所属线中的位置）
        data.pointInLineIndices.push_back(ptInLineCounter);

        // 保存线索引
        if (lineIdx >= 0) {
            data.lineIndices.push_back(lineIdx);
            if (lineIdx + 1 > data.totalLines) {
                data.totalLines = lineIdx + 1;
            }
        }

        ptInLineCounter++;
    }

    // 计算每线点数（假设网格化点云）
    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;
    data.hasCustomPointSizes = false;

    // 用于按点大小分组的临时 map
    std::map<float, std::vector<size_t>> sizeGroupMap;

    const float EPSILON = 1e-6f;
    int prevLineIdx = -1;
    int ptInLineCounter = 0;

    for (size_t i = 0; i < cloud.points.size(); ++i) {
        const auto& pt = cloud.points[i];

        // 跟踪线内索引（对所有点递增，包括被过滤的零点）
        int lineIdx = -1;
        if (data.hasLineInfo && i < cloud.lineIndices.size()) {
            lineIdx = cloud.lineIndices[i];
        }
        if (lineIdx != prevLineIdx) {
            ptInLineCounter = 0;
            prevLineIdx = lineIdx;
        }

        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            ptInLineCounter++;
            continue;
        }

        // 显示时过滤 (0,0,0) 点
        if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
            ptInLineCounter++;
            continue;
        }

        size_t pointIndex = data.vertices.size() / 3;

        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));

        // 保存线内索引（点在所属线中的位置）
        data.pointInLineIndices.push_back(ptInLineCounter);

        // 保存线索引
        if (lineIdx >= 0) {
            data.lineIndices.push_back(lineIdx);
            if (lineIdx + 1 > data.totalLines) {
                data.totalLines = lineIdx + 1;
            }
        }

        // 收集自定义点大小（A > 1）
        if (pt.pointSize > 1.0f) {
            sizeGroupMap[pt.pointSize].push_back(pointIndex);
        }

        ptInLineCounter++;
    }

    // 构建自定义点大小分组
    if (!sizeGroupMap.empty()) {
        data.hasCustomPointSizes = true;
        for (auto& pair : sizeGroupMap) {
            PointCloudData::PointSizeGroup group;
            group.pointSize = pair.first;
            group.indices = std::move(pair.second);
            data.customPointSizeGroups.push_back(std::move(group));
        }
        LOG_INFO("[CloudView] Found %zu custom point size groups\n", data.customPointSizeGroups.size());
    }

    // 计算每线点数
    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::appendPointCloud(const PointCloudXYZRGB& cloud,
                                          const QString& name)
{
    if (cloud.empty()) {
        return;
    }
    if (m_pointClouds.empty() || !m_pointClouds.back().hasColor) {
        addPointCloud(cloud, name);
        return;
    }

    PointCloudData& data = m_pointClouds.back();
    if (!name.isEmpty()) {
        data.name = name;
    }

    const bool incomingHasLineInfo = !cloud.lineIndices.empty();
    data.hasLineInfo = data.hasLineInfo || incomingHasLineInfo;
    const size_t originalIndexOffset = data.vertices.size() / 3;
    const float epsilon = 1e-6f;
    int previousLineIndex = -1;
    int pointInLineIndex = 0;

    for (size_t index = 0; index < cloud.points.size(); ++index) {
        const Point3DRGB& point = cloud.points[index];
        int lineIndex = -1;
        if (incomingHasLineInfo && index < cloud.lineIndices.size()) {
            lineIndex = cloud.lineIndices[index];
        }
        if (lineIndex != previousLineIndex) {
            pointInLineIndex = 0;
            previousLineIndex = lineIndex;
        }

        if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
            !std::isfinite(point.z) ||
            (std::fabs(point.x) < epsilon &&
             std::fabs(point.y) < epsilon &&
             std::fabs(point.z) < epsilon)) {
            ++pointInLineIndex;
            continue;
        }

        const size_t pointIndex = data.vertices.size() / 3;
        data.vertices.push_back(point.x);
        data.vertices.push_back(point.y);
        data.vertices.push_back(point.z);
        data.colors.push_back(point.r / 255.0f);
        data.colors.push_back(point.g / 255.0f);
        data.colors.push_back(point.b / 255.0f);
        data.originalIndices.push_back(
            static_cast<int>(originalIndexOffset + index));
        data.pointInLineIndices.push_back(pointInLineIndex);

        if (lineIndex >= 0) {
            data.lineIndices.push_back(lineIndex);
            data.totalLines = qMax(data.totalLines, lineIndex + 1);
        }

        if (point.pointSize > 1.0f) {
            auto sizeGroup = std::find_if(
                data.customPointSizeGroups.begin(),
                data.customPointSizeGroups.end(),
                [&point](const PointCloudData::PointSizeGroup& group) {
                    return group.pointSize == point.pointSize;
                });
            if (sizeGroup == data.customPointSizeGroups.end()) {
                PointCloudData::PointSizeGroup group;
                group.pointSize = point.pointSize;
                data.customPointSizeGroups.push_back(std::move(group));
                sizeGroup = data.customPointSizeGroups.end() - 1;
            }
            sizeGroup->indices.push_back(pointIndex);
            data.hasCustomPointSizes = true;
        }
        ++pointInLineIndex;
    }

    if (data.totalLines > 0) {
        data.pointsPerLine =
            static_cast<int>(data.vertices.size() / 3) / data.totalLines;
    }

    makeCurrent();
    uploadToVBO(data);
    doneCurrent();

    computeBoundingBox();
    m_distance = qMax(m_distance, fittedCameraDistance());
    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_basicShapeSegments.clear();
    m_basicShapeSurfaces.clear();
    m_basicShapePoints.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());

    // 默认视角：面向 XY 平面，X 向右，Y 向下
    // 绕 X 轴旋转 180°：Y 轴从向上翻转为向下
    m_rotationX = 180.0f;
    m_rotationY = 0.0f;
    m_rotationZ = 0.0f;
    m_rotation = QQuaternion::fromEulerAngles(m_rotationX, m_rotationY, m_rotationZ);
    m_pan = QVector3D(0, 0, 0);
    m_distance = fittedCameraDistance();

    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::fitToContent()
{
    computeBoundingBox();
    resetView();
}

void PointCloudGLWidget::updateProjection(const QMatrix4x4& modelView)
{
    const QVector3D corners[] = {
        {m_minBound.x(), m_minBound.y(), m_minBound.z()},
        {m_minBound.x(), m_minBound.y(), m_maxBound.z()},
        {m_minBound.x(), m_maxBound.y(), m_minBound.z()},
        {m_minBound.x(), m_maxBound.y(), m_maxBound.z()},
        {m_maxBound.x(), m_minBound.y(), m_minBound.z()},
        {m_maxBound.x(), m_minBound.y(), m_maxBound.z()},
        {m_maxBound.x(), m_maxBound.y(), m_minBound.z()},
        {m_maxBound.x(), m_maxBound.y(), m_maxBound.z()}
    };

    float minimumDepth = FLT_MAX;
    float maximumDepth = -FLT_MAX;
    for (const QVector3D& corner : corners) {
        const float depth = -modelView.map(corner).z();
        minimumDepth = qMin(minimumDepth, depth);
        maximumDepth = qMax(maximumDepth, depth);
    }

    constexpr float kMinimumNearPlane = 0.01f;
    float nearPlane = kMinimumNearPlane;
    float farPlane = 1.0f;
    if (maximumDepth > kMinimumNearPlane) {
        // 点云贴近或跨过相机时保持很小的近裁剪距离，避免前侧点被裁掉。
        if (minimumDepth > kMinimumNearPlane) {
            nearPlane = qMax(kMinimumNearPlane, minimumDepth * 0.5f);
        }
        // 远裁剪面仅覆盖当前内容，并限制 near/far 比值以保证深度缓冲精度。
        farPlane = qMax(nearPlane + 1.0f, maximumDepth * 1.5f);
        nearPlane = qMax(nearPlane, farPlane / 100000.0f);
    }

    const float aspect = static_cast<float>(width()) /
                         static_cast<float>(height() > 0 ? height() : 1);
    m_projection.setToIdentity();
    m_projection.perspective(45.0f, aspect, nearPlane, farPlane);
}

float PointCloudGLWidget::fittedCameraDistance(float padding) const
{
    const QVector3D corners[] = {
        {m_minBound.x(), m_minBound.y(), m_minBound.z()},
        {m_minBound.x(), m_minBound.y(), m_maxBound.z()},
        {m_minBound.x(), m_maxBound.y(), m_minBound.z()},
        {m_minBound.x(), m_maxBound.y(), m_maxBound.z()},
        {m_maxBound.x(), m_minBound.y(), m_minBound.z()},
        {m_maxBound.x(), m_minBound.y(), m_maxBound.z()},
        {m_maxBound.x(), m_maxBound.y(), m_minBound.z()},
        {m_maxBound.x(), m_maxBound.y(), m_maxBound.z()}};

    const float aspect = qMax(0.01f,
                              static_cast<float>(width()) /
                                  static_cast<float>(height() > 0 ? height() : 1));
    const float tangent = std::tan(qDegreesToRadians(45.0f * 0.5f));
    const float safePadding = qMax(1.0f, padding);
    float distance = minimumCameraDistance();
    for (const QVector3D& corner : corners) {
        const QVector3D relative =
            m_rotation.rotatedVector(corner - m_center);
        const float horizontalDistance =
            relative.z() + std::fabs(relative.x()) * safePadding / (tangent * aspect);
        const float verticalDistance =
            relative.z() + std::fabs(relative.y()) * safePadding / tangent;
        distance = qMax(distance, qMax(horizontalDistance, verticalDistance));
    }
    return qBound(10.0f, distance, 1.0e7f);
}

float PointCloudGLWidget::minimumCameraDistance() const
{
    const QVector3D corners[] = {
        {m_minBound.x(), m_minBound.y(), m_minBound.z()},
        {m_minBound.x(), m_minBound.y(), m_maxBound.z()},
        {m_minBound.x(), m_maxBound.y(), m_minBound.z()},
        {m_minBound.x(), m_maxBound.y(), m_maxBound.z()},
        {m_maxBound.x(), m_minBound.y(), m_minBound.z()},
        {m_maxBound.x(), m_minBound.y(), m_maxBound.z()},
        {m_maxBound.x(), m_maxBound.y(), m_minBound.z()},
        {m_maxBound.x(), m_maxBound.y(), m_maxBound.z()}
    };

    float frontExtent = 0.0f;
    for (const QVector3D& corner : corners) {
        const QVector3D relativePosition = corner - m_center;
        frontExtent = qMax(frontExtent,
                           m_rotation.rotatedVector(relativePosition).z());
    }

    const float margin = qMax(0.1f, (m_maxBound - m_minBound).length() * 0.002f);
    return qBound(0.1f, frontExtent + margin, 1.0e7f);
}

void PointCloudGLWidget::zoomIn()
{
    m_distance *= 0.9f;
    m_distance = qBound(minimumCameraDistance(), m_distance, 1.0e7f);
    update();
}

void PointCloudGLWidget::zoomOut()
{
    m_distance *= 1.1f;
    m_distance = qBound(minimumCameraDistance(), m_distance, 1.0e7f);
    update();
}

void PointCloudGLWidget::setViewAngles(float rotX, float rotY, float rotZ)
{
    // 归一化到 ±180°，避免往同一方向旋转时角度无限累加
    m_rotationX = NormalizeAngleDegrees(rotX);
    m_rotationY = NormalizeAngleDegrees(rotY);
    m_rotationZ = NormalizeAngleDegrees(rotZ);
    m_pan = QVector3D(0, 0, 0);

    // 从欧拉角转换为四元数（ZYX顺序）
    m_rotation = QQuaternion::fromEulerAngles(m_rotationX, m_rotationY, m_rotationZ);
    m_distance = qMax(m_distance, minimumCameraDistance());

    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);
}

void PointCloudGLWidget::updateSelectedPointCoord(int index, float x, float y, float z)
{
    if (index < 0 || index >= m_selectedPoints.size()) {
        return;
    }

    if (!m_selectedPoints[index].valid) {
        return;
    }

    // 更新选中点的坐标
    m_selectedPoints[index].x = x;
    m_selectedPoints[index].y = y;
    m_selectedPoints[index].z = z;

    LOG_INFO("[CloudView] Updated selected point %d to (%.3f, %.3f, %.3f)\n", index, x, y, z);

    // 刷新显示
    update();
}

void PointCloudGLWidget::setSelectedPointCoord(int index, float x, float y, float z)
{
    if (index < 0 || index >= MAX_SELECTED_POINTS) {
        return;
    }

    // 如果索引位置不存在，扩展列表
    while (m_selectedPoints.size() <= index) {
        m_selectedPoints.append(SelectedPointInfo());
    }

    m_selectedPoints[index].valid = true;
    m_selectedPoints[index].x = x;
    m_selectedPoints[index].y = y;
    m_selectedPoints[index].z = z;
    m_selectedPoints[index].cloudIndex = -1;
    m_selectedPoints[index].lineIndex = -1;
    m_selectedPoints[index].pointIndexInLine = -1;

    LOG_INFO("[CloudView] Set selected point %d to (%.3f, %.3f, %.3f)\n", index, x, y, z);

    // 刷新显示
    update();
}

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();
}

size_t PointCloudGLWidget::rotateCloudsByZThreshold(const QMatrix4x4& rotMatrix, float zThreshold)
{
    size_t rotatedCount = 0;

    for (auto& cloudData : m_pointClouds) {
        for (size_t i = 0; i + 2 < cloudData.vertices.size(); i += 3) {
            if (cloudData.vertices[i + 2] < zThreshold) {
                QVector3D pt(cloudData.vertices[i], cloudData.vertices[i + 1], cloudData.vertices[i + 2]);
                QVector3D transformed = rotMatrix.map(pt);
                cloudData.vertices[i] = transformed.x();
                cloudData.vertices[i + 1] = transformed.y();
                cloudData.vertices[i + 2] = transformed.z();
                ++rotatedCount;
            }
        }
    }

    if (rotatedCount > 0) {
        makeCurrent();
        for (auto& cloudData : m_pointClouds) {
            uploadToVBO(cloudData);
        }
        doneCurrent();
        computeBoundingBox();
        resetView();
        update();
    }

    return rotatedCount;
}

size_t PointCloudGLWidget::getAllCloudsByLines(std::vector<std::vector<SVzNL3DPosition>>& scanLines) const
{
    scanLines.clear();
    size_t totalPoints = 0;

    for (const auto& cloudData : m_pointClouds) {
        const size_t displayCount = cloudData.vertices.size() / 3;
        if (displayCount == 0) continue;

        if (cloudData.hasLineInfo && cloudData.totalLines > 0 && cloudData.pointsPerLine > 0) {
            // 有线信息：用 pointsPerLine 还原每条线的完整点数，过滤掉的 (0,0,0) 点补零
            const int lines = cloudData.totalLines;
            const int ptsPerLine = cloudData.pointsPerLine;

            // 初始化所有线，每条线 pointsPerLine 个零点
            const size_t baseIdx = scanLines.size();
            for (int i = 0; i < lines; ++i) {
                std::vector<SVzNL3DPosition> line(ptsPerLine);
                memset(line.data(), 0, sizeof(SVzNL3DPosition) * ptsPerLine);
                for (int j = 0; j < ptsPerLine; ++j) {
                    line[j].nPointIdx = j;
                }
                scanLines.push_back(std::move(line));
            }

            // 用实际显示的点填充对应位置
            for (size_t i = 0; i < displayCount; ++i) {
                const int lineIdx = (i < cloudData.lineIndices.size()) ? cloudData.lineIndices[i] : 0;
                const int ptInLine = (i < cloudData.pointInLineIndices.size()) ? cloudData.pointInLineIndices[i] : static_cast<int>(i);

                if (lineIdx >= 0 && lineIdx < lines && ptInLine >= 0 && ptInLine < ptsPerLine) {
                    auto& pos = scanLines[baseIdx + lineIdx][ptInLine];
                    pos.pt3D.x = cloudData.vertices[i * 3];
                    pos.pt3D.y = cloudData.vertices[i * 3 + 1];
                    pos.pt3D.z = cloudData.vertices[i * 3 + 2];
                    pos.nPointIdx = ptInLine;
                }
            }

            totalPoints += static_cast<size_t>(lines) * ptsPerLine;
        } else {
            // 无线信息，每个点云作为一条线
            std::vector<SVzNL3DPosition> line;
            line.reserve(displayCount);
            for (size_t i = 0; i < displayCount; ++i) {
                SVzNL3DPosition pos;
                memset(&pos, 0, sizeof(pos));
                pos.nPointIdx = static_cast<int>(i);
                pos.pt3D.x = cloudData.vertices[i * 3];
                pos.pt3D.y = cloudData.vertices[i * 3 + 1];
                pos.pt3D.z = cloudData.vertices[i * 3 + 2];
                line.push_back(pos);
            }
            totalPoints += displayCount;
            scanLines.push_back(std::move(line));
        }
    }

    return totalPoints;
}

void PointCloudGLWidget::computeBoundingBox()
{
    float minX = FLT_MAX, minY = FLT_MAX, minZ = FLT_MAX;
    float maxX = -FLT_MAX, maxY = -FLT_MAX, maxZ = -FLT_MAX;
    bool hasContent = false;

    auto includePoint = [&](float x, float y, float z) {
        minX = qMin(minX, x);
        minY = qMin(minY, y);
        minZ = qMin(minZ, z);
        maxX = qMax(maxX, x);
        maxY = qMax(maxY, y);
        maxZ = qMax(maxZ, z);
        hasContent = true;
    };

    auto includeSegment = [&](const LineSegment& segment) {
        includePoint(segment.x1, segment.y1, segment.z1);
        includePoint(segment.x2, segment.y2, segment.z2);
    };

    for (const auto& cloudData : m_pointClouds) {
        for (size_t i = 0; i < cloudData.vertices.size(); i += 3) {
            includePoint(cloudData.vertices[i], cloudData.vertices[i + 1], cloudData.vertices[i + 2]);
        }
    }

    for (const LineSegment& segment : m_lineSegments) {
        includeSegment(segment);
    }

    for (const LineSegment& segment : m_basicShapeSegments) {
        includeSegment(segment);
    }

    for (const SurfaceQuad& surface : m_basicShapeSurfaces) {
        includePoint(surface.p1.x(), surface.p1.y(), surface.p1.z());
        includePoint(surface.p2.x(), surface.p2.y(), surface.p2.z());
        includePoint(surface.p3.x(), surface.p3.y(), surface.p3.z());
        includePoint(surface.p4.x(), surface.p4.y(), surface.p4.z());
    }

    for (const BasicShapePoint& point : m_basicShapePoints) {
        includePoint(point.x, point.y, point.z);
    }

    for (const PosePoint& pose : m_posePoints) {
        const float scale = qMax(0.0f, pose.scale);
        includePoint(pose.x - scale, pose.y - scale, pose.z - scale);
        includePoint(pose.x + scale, pose.y + scale, pose.z + scale);
    }

    if (!hasContent) {
        m_minBound = QVector3D(-50, -50, -50);
        m_maxBound = QVector3D(50, 50, 50);
        m_center = QVector3D(0, 0, 0);
        return;
    }

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

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

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

    // 视口（Qt 窗口即视口）；GLES2 无 GL_MODELVIEW_MATRIX / gluProject，用与渲染一致的矩阵手动投影
    const GLint viewport[4] = {0, 0, static_cast<GLint>(width()), static_cast<GLint>(height())};
    const QMatrix4x4 modelView = m_view * m_model;
    const QMatrix4x4 mvp = m_projection * modelView;

    // 转换屏幕 Y 坐标（OpenGL Y 轴向上）
    const 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) {
            const float wx = cloudData.vertices[i];
            const float wy = cloudData.vertices[i + 1];
            const float wz = cloudData.vertices[i + 2];

            // 将世界坐标投影到屏幕（等价于 gluProject）
            const QVector4D clip = mvp * QVector4D(wx, wy, wz, 1.0f);
            if (std::fabs(clip.w()) < 1e-9f) {
                continue;
            }
            const float ndcX = clip.x() / clip.w();
            const float ndcY = clip.y() / clip.w();
            const float sx = viewport[0] + (ndcX + 1.0f) * 0.5f * viewport[2];
            const float sy = viewport[1] + (ndcY + 1.0f) * 0.5f * viewport[3];

            // 计算屏幕距离
            const float dx = sx - screenX;
            const float dy = sy - glScreenY;
            const 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.pointInLineIndices.size()) {
                result.pointIndexInLine = cloudData.pointInLineIndices[bestIndex];
            }
        }

        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()
{
    QVector<float> positions;
    QVector<float> colors;

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

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

    if (positions.isEmpty()) {
        return;
    }
    drawPrimitives(m_mvp,
                   positions.constData(),
                   positions.size() / 3,
                   GL_POINTS,
                   colors.constData(),
                   QVector3D(1.0f, 1.0f, 1.0f),
                   10.0f,
                   1.0f,
                   false);
}

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;
    }

    const float positions[] = {
        p1.x, p1.y, p1.z,
        p2.x, p2.y, p2.z};
    drawPrimitives(m_mvp,
                   positions,
                   2,
                   GL_LINES,
                   nullptr,
                   QVector3D(0.0f, 1.0f, 0.0f),
                   1.0f,
                   1.0f,
                   false);
}

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;
    }

    // 高亮显示选中线上的所有点（黄色）
    QVector<float> positions;

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

    if (positions.isEmpty()) {
        return;
    }
    drawPrimitives(m_mvp,
                   positions.constData(),
                   positions.size() / 3,
                   GL_POINTS,
                   nullptr,
                   QVector3D(1.0f, 1.0f, 0.0f),
                   3.0f,
                   1.0f,
                   false);
}

void PointCloudGLWidget::drawAxis()
{
    // 在右下角绘制坐标系指示器
    // 坐标系定义：X向右，Y向下，Z朝后
    // 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 轴向上）

    // 正交投影 + 视图变换（等价于原 glOrtho + glTranslatef + glMultMatrixf）
    QMatrix4x4 projection;
    projection.setToIdentity();
    projection.ortho(0, viewport[2], 0, viewport[3], -100, 100);

    QMatrix4x4 modelView;
    modelView.setToIdentity();
    modelView.translate(axisX + axisSize / 2.0f, axisY + axisSize / 2.0f, 0.0f);
    QMatrix4x4 rotation;
    rotation.rotate(m_rotation);
    modelView *= rotation;

    const QMatrix4x4 mvp = projection * modelView;

    const float axisLength = axisSize * 0.4f;  // 坐标轴长度
    const float positions[] = {
        0.0f, 0.0f, 0.0f,  axisLength, 0.0f, 0.0f,    // X 轴 - 红色（向右）
        0.0f, 0.0f, 0.0f,  0.0f, axisLength, 0.0f,    // Y 轴 - 绿色（向下）
        0.0f, 0.0f, 0.0f,  0.0f, 0.0f, axisLength     // Z 轴 - 蓝色（向后）
    };
    const float colors[] = {
        1.0f, 0.0f, 0.0f,  1.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f,  0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 1.0f,  0.0f, 0.0f, 1.0f
    };

    // 关闭深度测试，确保坐标系始终可见
    drawPrimitives(mvp, positions, 6, GL_LINES, colors,
                   QVector3D(1.0f, 1.0f, 1.0f), 1.0f, 1.0f, false);
}

void PointCloudGLWidget::drawAxisLabels()
{
    GLint viewport[4];
    glGetIntegerv(GL_VIEWPORT, viewport);

    int axisSize = 60;
    int margin = 10;
    float axisLength = axisSize * 0.4f;

    // 坐标轴中心在 OpenGL 坐标系中的位置
    float centerGlX = viewport[2] - axisSize / 2.0f - margin;
    float centerGlY = axisSize / 2.0f + margin;

    // 对各轴端点应用当前视图旋转
    QMatrix4x4 rotMatrix;
    rotMatrix.rotate(m_rotation);

    QVector3D xEnd = rotMatrix.map(QVector3D(axisLength, 0, 0));
    QVector3D yEnd = rotMatrix.map(QVector3D(0, axisLength, 0));
    QVector3D zEnd = rotMatrix.map(QVector3D(0, 0, axisLength));

    // 转换为 Qt widget 坐标（Y 轴翻转：OpenGL Y 向上 → Qt Y 向下）
    auto toWidgetPos = [&](const QVector3D& v) -> QPointF {
        float glX = centerGlX + v.x();
        float glY = centerGlY + v.y();
        return QPointF(glX, viewport[3] - glY);
    };

    QPointF xPos = toWidgetPos(xEnd);
    QPointF yPos = toWidgetPos(yEnd);
    QPointF zPos = toWidgetPos(zEnd);

    // 使用 QPainter 绘制文字标注
    QPainter painter(this);
    painter.setRenderHint(QPainter::TextAntialiasing);
    QFont font = painter.font();
    font.setBold(true);
    font.setPointSize(14);
    painter.setFont(font);

    // 文字偏移：向轴方向外侧偏移，避免与轴线重叠
    int offset = 4;

    painter.setPen(QColor(255, 80, 80));    // 红色
    painter.drawText(xPos.x() + offset, xPos.y() + offset, "X");

    painter.setPen(QColor(80, 255, 80));    // 绿色
    painter.drawText(yPos.x() + offset, yPos.y() + offset, "Y");

    painter.setPen(QColor(80, 128, 255));   // 蓝色
    painter.drawText(zPos.x() + offset, zPos.y() + offset, "Z");

    painter.end();
}

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) {
                        const float distance = calculateDistance(m_selectedPoints[0], m_selectedPoints[1]);
                        const float dx = m_selectedPoints[1].x - m_selectedPoints[0].x;
                        const float dy = m_selectedPoints[1].y - m_selectedPoints[0].y;
                        const float dz = m_selectedPoints[1].z - m_selectedPoints[0].z;
                        emit twoPointsSelected(m_selectedPoints[0], m_selectedPoints[1], distance);
                        emit measurementFinished(m_selectedPoints[0], m_selectedPoints[1], distance, dx, dy, dz);
                    }
                } 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::mouseDoubleClickEvent(QMouseEvent* event)
{
    if (event->button() == Qt::LeftButton) {
        // 双击左键：选点（与 Ctrl+左键 相同逻辑）
        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) {
                    const float distance = calculateDistance(m_selectedPoints[0], m_selectedPoints[1]);
                    const float dx = m_selectedPoints[1].x - m_selectedPoints[0].x;
                    const float dy = m_selectedPoints[1].y - m_selectedPoints[0].y;
                    const float dz = m_selectedPoints[1].z - m_selectedPoints[0].z;
                    emit twoPointsSelected(m_selectedPoints[0], m_selectedPoints[1], distance);
                    emit measurementFinished(m_selectedPoints[0], m_selectedPoints[1], distance, dx, dy, dz);
                }
            } else {
                // 未启用测距：只保留一个点
                m_selectedPoints.clear();
                m_selectedPoints.append(point);
                emit pointSelected(point);
            }

            update();
        }
    }
}

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

    if (m_leftButtonPressed) {
        // Alt+左键拖动：绕视线方向旋转（滚转）
        if (event->modifiers() & Qt::AltModifier) {
            // 绕Z轴（视线方向）旋转（修正方向）
            float angle = -delta.x() * 0.5f;  // 取反修正方向
            QQuaternion deltaRotation = QQuaternion::fromAxisAndAngle(QVector3D(0, 0, 1), angle);
            m_rotation = deltaRotation * m_rotation;
            m_rotationZ = NormalizeAngleDegrees(m_rotationZ + angle);
        } else {
            // 普通左键拖动：基于当前物体坐标系的旋转
            // 鼠标水平移动 -> 绕当前Y轴旋转
            // 鼠标垂直移动 -> 绕当前X轴旋转

            // 计算旋转角度
            float angleX = delta.y() * 0.5f;  // 垂直移动
            float angleY = delta.x() * 0.5f;  // 水平移动

            // 创建增量旋转四元数（在相机空间中）
            // 先绕X轴旋转（俯仰），再绕Y轴旋转（偏航）
            QQuaternion deltaRotationX = QQuaternion::fromAxisAndAngle(QVector3D(1, 0, 0), angleX);
            QQuaternion deltaRotationY = QQuaternion::fromAxisAndAngle(QVector3D(0, 1, 0), angleY);
            QQuaternion deltaRotation = deltaRotationY * deltaRotationX;

            // 应用增量旋转（左乘，相机空间旋转）
            m_rotation = deltaRotation * m_rotation;

            // 更新欧拉角（用于显示），归一化到 ±180°
            m_rotationX = NormalizeAngleDegrees(m_rotationX + angleX);
            m_rotationY = NormalizeAngleDegrees(m_rotationY + angleY);
        }

        // 发送角度变化信号
        m_distance = qMax(m_distance, minimumCameraDistance());
        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) {
        // 右键拖动：绕视线方向旋转（滚转）（修正方向）
        float angle = -delta.x() * 0.5f;  // 取反修正方向
        QQuaternion deltaRotation = QQuaternion::fromAxisAndAngle(QVector3D(0, 0, 1), angle);
        m_rotation = deltaRotation * m_rotation;
        m_rotationZ = NormalizeAngleDegrees(m_rotationZ + angle);
        m_distance = qMax(m_distance, minimumCameraDistance());
        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(minimumCameraDistance(), m_distance, 1.0e7f);
    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::setBasicShapeSegments(const QVector<LineSegment>& segments)
{
    m_basicShapeSegments = segments;
    update();
}

void PointCloudGLWidget::appendBasicShapeSegments(const QVector<LineSegment>& segments)
{
    if (segments.isEmpty()) {
        return;
    }

    for (const LineSegment& segment : segments) {
        m_basicShapeSegments.append(segment);
    }
    update();
}

void PointCloudGLWidget::appendBasicShapeSurfaces(const QVector<SurfaceQuad>& surfaces)
{
    if (surfaces.isEmpty()) {
        return;
    }

    for (const SurfaceQuad& surface : surfaces) {
        m_basicShapeSurfaces.append(surface);
    }
    update();
}

void PointCloudGLWidget::appendBasicShapePoints(const QVector<BasicShapePoint>& points)
{
    if (points.isEmpty()) {
        return;
    }

    for (const BasicShapePoint& point : points) {
        m_basicShapePoints.append(point);
    }
    update();
}

void PointCloudGLWidget::clearBasicShapeSegments()
{
    m_basicShapeSegments.clear();
    m_basicShapeSurfaces.clear();
    m_basicShapePoints.clear();
    update();
}

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

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

void PointCloudGLWidget::drawLineSegments()
{
    drawLineSegmentList(m_lineSegments);
}

void PointCloudGLWidget::drawBasicShapeSurfaces()
{
    if (m_basicShapeSurfaces.isEmpty()) {
        return;
    }

    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glDepthMask(GL_FALSE);

    for (const SurfaceQuad& surface : m_basicShapeSurfaces) {
        // GLES2 无 GL_QUADS，四边形拆成两个三角形 (p1,p2,p3) 和 (p1,p3,p4)
        const float positions[] = {
            surface.p1.x(), surface.p1.y(), surface.p1.z(),
            surface.p2.x(), surface.p2.y(), surface.p2.z(),
            surface.p3.x(), surface.p3.y(), surface.p3.z(),
            surface.p1.x(), surface.p1.y(), surface.p1.z(),
            surface.p3.x(), surface.p3.y(), surface.p3.z(),
            surface.p4.x(), surface.p4.y(), surface.p4.z(),
        };
        drawPrimitives(m_mvp, positions, 6, GL_TRIANGLES, nullptr,
                       QVector3D(surface.r, surface.g, surface.b),
                       1.0f, surface.a, true);
    }

    glDepthMask(GL_TRUE);
    glDisable(GL_BLEND);
}

void PointCloudGLWidget::drawBasicShapeSegments()
{
    drawLineSegmentList(m_basicShapeSegments);
}

void PointCloudGLWidget::drawBasicShapePoints()
{
    if (m_basicShapePoints.isEmpty()) {
        return;
    }

    for (const BasicShapePoint& point : m_basicShapePoints) {
        const float positions[] = {point.x, point.y, point.z};
        drawPrimitives(m_mvp, positions, 1, GL_POINTS, nullptr,
                       QVector3D(point.r, point.g, point.b),
                       point.pointSize, 1.0f, false);
    }
}

void PointCloudGLWidget::drawLineSegmentList(const QVector<LineSegment>& segments)
{
    if (segments.isEmpty()) {
        return;
    }

    // 按线宽分组绘制
    // 收集所有不同的线宽值（0 表示默认 2.0f）
    std::map<float, QVector<int>> widthGroups;
    for (int i = 0; i < segments.size(); ++i) {
        const float w = segments[i].lineWidth > 0 ? segments[i].lineWidth : 2.0f;
        widthGroups[w].append(i);
    }

    for (const auto& group : widthGroups) {
        QVector<float> positions;
        QVector<float> colors;
        positions.reserve(group.second.size() * 6);
        colors.reserve(group.second.size() * 6);
        for (int idx : group.second) {
            const LineSegment& seg = segments[idx];
            positions.append(seg.x1); positions.append(seg.y1); positions.append(seg.z1);
            positions.append(seg.x2); positions.append(seg.y2); positions.append(seg.z2);
            colors.append(seg.r); colors.append(seg.g); colors.append(seg.b);
            colors.append(seg.r); colors.append(seg.g); colors.append(seg.b);
        }
        if (positions.isEmpty()) {
            continue;
        }
        // GLES2 下 glLineWidth 有效宽度常受限（部分 Mali 仅支持 1.0），
        // 仍按原线宽设置，驱动不支持时自动退化为最接近的可用宽度。
        glLineWidth(group.first);
        drawPrimitives(m_mvp, positions.constData(), positions.size() / 3,
                       GL_LINES, colors.constData(),
                       QVector3D(1.0f, 1.0f, 1.0f), 1.0f, 1.0f, true);
    }

    glLineWidth(1.0f);
}

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

    glLineWidth(2.0f);

    for (const auto& pose : m_posePoints) {
        // 移动到姿态点位置，并按欧拉角顺序旋转（QMatrix4x4::rotate 与 glRotatef 同为右乘）
        QMatrix4x4 modelView;
        modelView.setToIdentity();
        modelView.translate(pose.x, pose.y, pose.z);
        switch (m_eulerRotationOrder) {
        case EulerRotationOrder::ZYX:  // Yaw-Pitch-Roll（最常用）
            modelView.rotate(pose.rx, 1, 0, 0);
            modelView.rotate(pose.ry, 0, 1, 0);
            modelView.rotate(pose.rz, 0, 0, 1);
            break;
        case EulerRotationOrder::XYZ:  // Roll-Pitch-Yaw
            modelView.rotate(pose.rz, 0, 0, 1);
            modelView.rotate(pose.ry, 0, 1, 0);
            modelView.rotate(pose.rx, 1, 0, 0);
            break;
        case EulerRotationOrder::ZXY:  // Yaw-Roll-Pitch
            modelView.rotate(pose.ry, 0, 1, 0);
            modelView.rotate(pose.rx, 1, 0, 0);
            modelView.rotate(pose.rz, 0, 0, 1);
            break;
        case EulerRotationOrder::YXZ:  // Pitch-Roll-Yaw
            modelView.rotate(pose.rz, 0, 0, 1);
            modelView.rotate(pose.rx, 1, 0, 0);
            modelView.rotate(pose.ry, 0, 1, 0);
            break;
        case EulerRotationOrder::XZY:  // Roll-Yaw-Pitch
            modelView.rotate(pose.ry, 0, 1, 0);
            modelView.rotate(pose.rz, 0, 0, 1);
            modelView.rotate(pose.rx, 1, 0, 0);
            break;
        case EulerRotationOrder::YZX:  // Pitch-Yaw-Roll
            modelView.rotate(pose.rx, 1, 0, 0);
            modelView.rotate(pose.rz, 0, 0, 1);
            modelView.rotate(pose.ry, 0, 1, 0);
            break;
        }

        const QMatrix4x4 mvp = m_projection * modelView;

        // 绘制坐标系（右手坐标系：X红右，Y绿上，Z蓝前）
        const float positions[] = {
            0.0f, 0.0f, 0.0f,  pose.scale, 0.0f, 0.0f,    // X轴 - 红色
            0.0f, 0.0f, 0.0f,  0.0f, pose.scale, 0.0f,    // Y轴 - 绿色
            0.0f, 0.0f, 0.0f,  0.0f, 0.0f, pose.scale     // Z轴 - 蓝色
        };
        const float colors[] = {
            1.0f, 0.0f, 0.0f,  1.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f,  0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 1.0f,  0.0f, 0.0f, 1.0f
        };
        drawPrimitives(mvp, positions, 6, GL_LINES, colors,
                       QVector3D(1.0f, 1.0f, 1.0f), 1.0f, 1.0f, true);
    }

    glLineWidth(1.0f);
}