#include "gui/PointCloudGLWidget.h"
#include <QDebug>
#include <QPushButton>
#include <cmath>
#include <cfloat>

PointCloudGLWidget::PointCloudGLWidget(QWidget *parent)
    : QOpenGLWidget(parent)
    , m_program(nullptr)
    , m_vertexBuffer(nullptr)
    , m_vao(nullptr)
    , m_pointCount(0)
    , m_minZ(0.0f)
    , m_maxZ(1000.0f)
    , m_fov(60.0f)          // 透视投影视场角
    , m_rotationX(0.0f)     // 从正面看（0度）
    , m_rotationY(0.0f)     // 不旋转Y轴
    , m_cloudCenter(0.0f, 0.0f, 0.0f)
    , m_viewDistance(1000.0f)
    , m_panOffset(0.0f, 0.0f, 0.0f)
    , m_zoom(1.0f)          // 缩放因子
    , m_leftButtonPressed(false)
    , m_rightButtonPressed(false)
    , m_firstFrame(true)
    , m_colorMode(0)  // 默认黑白模式
{
    setMinimumSize(400, 400);

    // 添加重置视角按钮
    QPushButton *resetBtn = new QPushButton("重置", this);
    resetBtn->setFixedSize(60, 30);
    resetBtn->move(10, 10);
    resetBtn->setStyleSheet(
        "QPushButton { background-color: rgba(50, 50, 50, 180); color: white; border: 1px solid #555; border-radius: 4px; }"
        "QPushButton:hover { background-color: rgba(70, 70, 70, 200); }"
        "QPushButton:pressed { background-color: rgba(40, 40, 40, 220); }"
    );
    connect(resetBtn, &QPushButton::clicked, this, &PointCloudGLWidget::resetView);
}

PointCloudGLWidget::~PointCloudGLWidget()
{
    makeCurrent();
    if (m_vao) {
        m_vao->destroy();
        delete m_vao;
    }
    if (m_vertexBuffer) {
        m_vertexBuffer->destroy();
        delete m_vertexBuffer;
    }
    if (m_program) {
        delete m_program;
    }
    doneCurrent();
}

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

    glClearColor(0.1f, 0.1f, 0.15f, 1.0f);  // 深灰色背景
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_PROGRAM_POINT_SIZE);

    setupShaders();

    // 创建VAO和VBO
    m_vao = new QOpenGLVertexArrayObject(this);
    m_vao->create();

    m_vertexBuffer = new QOpenGLBuffer(QOpenGLBuffer::VertexBuffer);
    m_vertexBuffer->create();

    qDebug() << "[PointCloudGLWidget] OpenGL initialized";
}

void PointCloudGLWidget::setupShaders()
{
    m_program = new QOpenGLShaderProgram(this);

    // 顶点着色器
    const char *vertexShaderSource = R"(
        #version 330 core
        layout(location = 0) in vec3 position;
        uniform mat4 mvp;
        uniform float minZ;
        uniform float maxZ;
        out float depth;
        void main() {
            gl_Position = mvp * vec4(position, 1.0);
            gl_PointSize = 1.0;
            depth = (position.z - minZ) / (maxZ - minZ);
        }
    )";

    // 片段着色器 - 支持黑白和彩色两种模式
    const char *fragmentShaderSource = R"(
        #version 330 core
        in float depth;
        uniform int colorMode;
        out vec4 fragColor;
        void main() {
            float d = clamp(depth, 0.0, 1.0);
            if (colorMode == 0) {
                fragColor = vec4(1.0, 1.0, 1.0, 1.0);
            } else {
                vec3 color;
                if (d < 0.25) {
                    color = mix(vec3(0.0, 0.0, 1.0), vec3(0.0, 1.0, 1.0), d * 4.0);
                } else if (d < 0.5) {
                    color = mix(vec3(0.0, 1.0, 1.0), vec3(0.0, 1.0, 0.0), (d - 0.25) * 4.0);
                } else if (d < 0.75) {
                    color = mix(vec3(0.0, 1.0, 0.0), vec3(1.0, 1.0, 0.0), (d - 0.5) * 4.0);
                } else {
                    color = mix(vec3(1.0, 1.0, 0.0), vec3(1.0, 0.0, 0.0), (d - 0.75) * 4.0);
                }
                fragColor = vec4(color, 1.0);
            }
        }
    )";

    if (!m_program->addShaderFromSourceCode(QOpenGLShader::Vertex, vertexShaderSource)) {
        qDebug() << "[PointCloudGLWidget] Vertex shader error:" << m_program->log();
    }
    if (!m_program->addShaderFromSourceCode(QOpenGLShader::Fragment, fragmentShaderSource)) {
        qDebug() << "[PointCloudGLWidget] Fragment shader error:" << m_program->log();
    }
    if (!m_program->link()) {
        qDebug() << "[PointCloudGLWidget] Shader link error:" << m_program->log();
    } else {
        qDebug() << "[PointCloudGLWidget] Shaders compiled and linked successfully";
    }
}

void PointCloudGLWidget::resizeGL(int w, int h)
{
    m_projection.setToIdentity();

    // 使用正交投影，避免透视变形
    float aspect = float(w) / float(h);
    float orthoSize = m_viewDistance * 0.5f / m_zoom;
    m_projection.ortho(-orthoSize * aspect, orthoSize * aspect,
                       -orthoSize, orthoSize,
                       -50000.0f, 50000.0f);
}

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

    if (m_pointCount == 0 || !m_program) {
        return;
    }

    // 重新计算正交投影矩阵
    m_projection.setToIdentity();
    float aspect = float(width()) / float(height());
    float orthoSize = m_viewDistance * 0.5f / m_zoom;
    m_projection.ortho(-orthoSize * aspect, orthoSize * aspect,
                       -orthoSize, orthoSize,
                       -50000.0f, 50000.0f);

    // 设置view矩阵 - 轨道相机模式（围绕点云中心旋转）
    m_view.setToIdentity();
    // 1. 用户平移偏移
    m_view.translate(m_panOffset);
    // 2. 相机后退到观察距离
    m_view.translate(0.0f, 0.0f, -m_viewDistance);
    // 3. 应用旋转（围绕原点，即点云中心）
    m_view.rotate(m_rotationX, 1.0f, 0.0f, 0.0f);
    m_view.rotate(m_rotationY, 0.0f, 1.0f, 0.0f);
    // 4. 将点云中心移到原点
    m_view.translate(-m_cloudCenter);

    // 设置model矩阵
    m_model.setToIdentity();

    // 计算MVP矩阵
    QMatrix4x4 mvp = m_projection * m_view * m_model;

    // 绑定shader和设置uniform
    m_program->bind();
    m_program->setUniformValue("mvp", mvp);
    m_program->setUniformValue("minZ", m_minZ);
    m_program->setUniformValue("maxZ", m_maxZ);
    m_program->setUniformValue("colorMode", m_colorMode);

    // 绑定VAO和绘制
    m_vao->bind();
    glDrawArrays(GL_POINTS, 0, m_pointCount);
    m_vao->release();

    m_program->release();
}

void PointCloudGLWidget::mousePressEvent(QMouseEvent *event)
{
    m_lastMousePos = event->pos();
    if (event->button() == Qt::LeftButton) {
        m_leftButtonPressed = true;
    } else if (event->button() == Qt::RightButton) {
        m_rightButtonPressed = true;
    }
}

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

    if (m_leftButtonPressed) {
        // 左键：旋转
        m_rotationX += delta.y() * 0.5f;
        m_rotationY += delta.x() * 0.5f;
        update();
    } else if (m_rightButtonPressed) {
        // 右键：平移（根据观察距离调整平移速度）
        float scale = m_viewDistance * 0.002f;
        m_panOffset.setX(m_panOffset.x() + delta.x() * scale);
        m_panOffset.setY(m_panOffset.y() - delta.y() * scale);
        update();
    }
}

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

void PointCloudGLWidget::wheelEvent(QWheelEvent *event)
{
    // 滚轮：缩放（调整zoom因子）
    float delta = event->angleDelta().y() / 120.0f;
    m_zoom *= (1.0f + delta * 0.1f);
    m_zoom = qMax(0.1f, qMin(m_zoom, 10.0f));  // 范围：0.1-10倍

    update();
}

void PointCloudGLWidget::updatePointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    if (!cloud) {
        return;
    }

    // 过滤全零点并转换为顶点数组，同时计算包围盒
    m_vertices.clear();

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

    for (const auto& point : cloud->points) {
        if (point.z > 0.01f) {  // 过滤掉无效的零点
            const float displayZ = -point.z; // Flip front/back axis for viewer convention.
            m_vertices.push_back(point.x);
            m_vertices.push_back(-point.y);
            m_vertices.push_back(displayZ);

            // 更新包围盒
            if (point.x < minX) minX = point.x;
            if (point.x > maxX) maxX = point.x;
            if (point.y < minY) minY = point.y;
            if (point.y > maxY) maxY = point.y;
            if (displayZ < minZ) minZ = displayZ;
            if (displayZ > maxZ) maxZ = displayZ;
        }
    }

    m_pointCount = m_vertices.size() / 3;

    // 保存深度范围用于着色
    if (m_pointCount > 0) {
        m_minZ = minZ;
        m_maxZ = maxZ;
    }

    // 只在首帧时自动调整相机位置，之后保持用户交互状态
    if (m_pointCount > 0 && m_firstFrame) {
        // 计算点云中心
        float centerX = (minX + maxX) / 2.0f;
        float centerY = (minY + maxY) / 2.0f;
        float centerZ = (minZ + maxZ) / 2.0f;

        // 计算点云尺寸
        float depthRange = maxZ - minZ;
        float sizeX = maxX - minX;
        float sizeY = maxY - minY;
        float maxSize = std::max({sizeX, sizeY, depthRange});

        // 设置点云中心
        m_cloudCenter = QVector3D(centerX, centerY, centerZ);

        // 计算观察距离，让相机从外部观察点云
        m_viewDistance = maxSize * 1.5f;

        // 重置平移偏移和旋转角度
        m_panOffset = QVector3D(0.0f, 0.0f, 0.0f);
        m_rotationX = 0.0f;
        m_rotationY = 0.0f;

        // 设置缩放
        m_zoom = 1.0f;

        qDebug() << "[PointCloudGLWidget] 首帧自动居中 - 点云中心:" << centerX << centerY << centerZ
                 << "观察距离:" << m_viewDistance;
        m_firstFrame = false;  // 标记首帧已处理
    }

    updateBuffers();
    update();
}

void PointCloudGLWidget::resetView()
{
    // 重置所有视角参数到初始状态
    m_rotationX = 0.0f;
    m_rotationY = 0.0f;
    m_panOffset = QVector3D(0.0f, 0.0f, 0.0f);
    m_zoom = 1.0f;
    m_firstFrame = true;  // 标记为首帧，下次更新时会重新计算视角

    update();
    qDebug() << "[PointCloudGLWidget] 视角已重置";
}

void PointCloudGLWidget::updateBuffers()
{
    if (m_vertices.empty() || !m_vao || !m_vertexBuffer) {
        return;
    }

    makeCurrent();

    m_vao->bind();
    m_vertexBuffer->bind();
    m_vertexBuffer->allocate(m_vertices.data(), m_vertices.size() * sizeof(float));

    // 设置顶点属性指针
    glEnableVertexAttribArray(0);
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(float), nullptr);

    m_vertexBuffer->release();
    m_vao->release();

    doneCurrent();
}