#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_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_measureDistanceEnabled(false)
    , m_hasListHighlightPoint(false)
    , m_colorIndex(0)
{
    setFocusPolicy(Qt::StrongFocus);
    setMouseTracking(true);

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

PointCloudGLWidget::~PointCloudGLWidget()
{
}

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);
    m_view.rotate(m_rotationX, 1, 0, 0);
    m_view.rotate(m_rotationY, 0, 1, 0);
    m_view.translate(-m_center);

    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) {
        const auto& cloudData = m_pointClouds[cloudIdx];
        if (cloudData.vertices.empty()) {
            continue;
        }

        glEnableClientState(GL_VERTEX_ARRAY);
        glVertexPointer(3, GL_FLOAT, 0, cloudData.vertices.data());

        // 如果有原始颜色且选择显示原始颜色
        if (cloudData.hasColor && !cloudData.colors.empty()) {
            glEnableClientState(GL_COLOR_ARRAY);
            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();

    // 最后绘制坐标系指示器（覆盖在所有内容之上）
    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));

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

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

void PointCloudGLWidget::clearPointClouds()
{
    m_pointClouds.clear();
    m_selectedPoints.clear();
    m_selectedLine = SelectedLineInfo();
    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);
    m_rotationX = 0.0f;  // X朝右，Y朝上，Z面向使用者
    m_rotationY = 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);

    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::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()
{
    // 在右下角绘制坐标系指示器
    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_rotationX, 1, 0, 0);
    glRotatef(m_rotationY, 0, 1, 0);

    // 关闭深度测试，确保坐标系始终可见
    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) {
        m_rotationY += delta.x() * 0.5f;
        m_rotationX += delta.y() * 0.5f;
        // 移除角度限制，允许无限旋转
        update();
    } else if (m_rightButtonPressed || 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();
    }
}

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

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

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

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