GrabBag/Tools/VrEyeView/Src/VrEyeViewWidget.cpp

#include "VrEyeViewWidget.h"
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QGridLayout>
#include <QGroupBox>
#include <QLabel>
#include <QLineEdit>
#include <QMessageBox>
#include <QFileDialog>
#include <QFileInfo>
#include <QPainter>
#include <QScrollArea>
#include <cmath>

VrEyeViewWidget::VrEyeViewWidget(QWidget* parent)
    : QWidget(parent)
    , m_eyeDevice(nullptr)
    , m_detector(nullptr)
    , m_leftImageLabel(nullptr)
    , m_rightImageLabel(nullptr)
    , m_isConnected(false)
    , m_isCapturing(false)
    , m_imageWidth(0)
    , m_imageHeight(0)
    , m_hasNewImage(false)
{
    // 创建设备和检测器
    IVrEyeDevice::CreateObject(&m_eyeDevice);
    m_detector = CreateChessboardDetectorInstance();

    setupUI();

    // 初始化设备
    if (m_eyeDevice) {
        m_eyeDevice->InitDevice();
    }

    // 创建显示定时器
    m_displayTimer = new QTimer(this);
    connect(m_displayTimer, &QTimer::timeout, this, &VrEyeViewWidget::onUpdateDisplay);
    m_displayTimer->start(33); // 约30fps
}

VrEyeViewWidget::~VrEyeViewWidget()
{
    if (m_isCapturing) {
        onStopCapture();
    }

    if (m_isConnected) {
        onDisconnectCamera();
    }

    if (m_detector) {
        DestroyChessboardDetectorInstance(m_detector);
        m_detector = nullptr;
    }

    if (m_eyeDevice) {
        delete m_eyeDevice;
        m_eyeDevice = nullptr;
    }
}

void VrEyeViewWidget::setupUI()
{
    QVBoxLayout* mainLayout = new QVBoxLayout(this);

    // 左右目图像并排显示区域
    QHBoxLayout* imageLayout = new QHBoxLayout();

    // 左目图像
    QVBoxLayout* leftLayout = new QVBoxLayout();
    leftLayout->addWidget(new QLabel("左目", this));
    m_leftImageLabel = new QLabel(this);
    m_leftImageLabel->setMinimumSize(320, 240);
    m_leftImageLabel->setAlignment(Qt::AlignCenter);
    m_leftImageLabel->setStyleSheet("QLabel { background-color: black; }");
    leftLayout->addWidget(m_leftImageLabel);
    imageLayout->addLayout(leftLayout);

    // 右目图像
    QVBoxLayout* rightLayout = new QVBoxLayout();
    rightLayout->addWidget(new QLabel("右目", this));
    m_rightImageLabel = new QLabel(this);
    m_rightImageLabel->setMinimumSize(320, 240);
    m_rightImageLabel->setAlignment(Qt::AlignCenter);
    m_rightImageLabel->setStyleSheet("QLabel { background-color: black; }");
    rightLayout->addWidget(m_rightImageLabel);
    imageLayout->addLayout(rightLayout);

    mainLayout->addLayout(imageLayout, 1);

    // 控制面板
    QHBoxLayout* controlLayout = new QHBoxLayout();

    // 相机连接组
    QGroupBox* cameraGroup = new QGroupBox("相机连接", this);
    QVBoxLayout* cameraLayout = new QVBoxLayout(cameraGroup);

    QHBoxLayout* ipLayout = new QHBoxLayout();
    ipLayout->addWidget(new QLabel("相机IP:", this));
    m_editCameraIP = new QLineEdit("192.168.1.100", this);
    ipLayout->addWidget(m_editCameraIP);
    cameraLayout->addLayout(ipLayout);

    m_btnConnect = new QPushButton("连接", this);
    m_btnDisconnect = new QPushButton("断开", this);
    m_btnDisconnect->setEnabled(false);
    connect(m_btnConnect, &QPushButton::clicked, this, &VrEyeViewWidget::onConnectCamera);
    connect(m_btnDisconnect, &QPushButton::clicked, this, &VrEyeViewWidget::onDisconnectCamera);

    QHBoxLayout* connectLayout = new QHBoxLayout();
    connectLayout->addWidget(m_btnConnect);
    connectLayout->addWidget(m_btnDisconnect);
    cameraLayout->addLayout(connectLayout);

    controlLayout->addWidget(cameraGroup);

    // 相机参数组
    QGroupBox* paramGroup = new QGroupBox("相机参数", this);
    QGridLayout* paramLayout = new QGridLayout(paramGroup);

    paramLayout->addWidget(new QLabel("曝光:", this), 0, 0);
    m_sbExposure = new QSpinBox(this);
    m_sbExposure->setRange(100, 10000);
    m_sbExposure->setValue(1000);
    paramLayout->addWidget(m_sbExposure, 0, 1);

    paramLayout->addWidget(new QLabel("增益:", this), 1, 0);
    m_sbGain = new QSpinBox(this);
    m_sbGain->setRange(0, 255);
    m_sbGain->setValue(100);
    paramLayout->addWidget(m_sbGain, 1, 1);

    controlLayout->addWidget(paramGroup);

    // 采集控制组
    QGroupBox* captureGroup = new QGroupBox("采集控制", this);
    QVBoxLayout* captureLayout = new QVBoxLayout(captureGroup);

    m_btnStartCapture = new QPushButton("开始采集", this);
    m_btnStopCapture = new QPushButton("停止采集", this);
    QPushButton* btnLoadImage = new QPushButton("加载图片", this);
    m_btnStartCapture->setEnabled(false);
    m_btnStopCapture->setEnabled(false);
    connect(m_btnStartCapture, &QPushButton::clicked, this, &VrEyeViewWidget::onStartCapture);
    connect(m_btnStopCapture, &QPushButton::clicked, this, &VrEyeViewWidget::onStopCapture);
    connect(btnLoadImage, &QPushButton::clicked, this, &VrEyeViewWidget::onLoadImage);

    captureLayout->addWidget(m_btnStartCapture);
    captureLayout->addWidget(m_btnStopCapture);
    captureLayout->addWidget(btnLoadImage);

    controlLayout->addWidget(captureGroup);

    mainLayout->addLayout(controlLayout);

    // 标定板检测参数
    QGroupBox* detectionGroup = new QGroupBox("标定板检测", this);
    QGridLayout* detectionLayout = new QGridLayout(detectionGroup);

    detectionLayout->addWidget(new QLabel("内角点宽度:", this), 0, 0);
    m_sbPatternWidth = new QSpinBox(this);
    m_sbPatternWidth->setRange(2, 20);
    m_sbPatternWidth->setValue(8);
    detectionLayout->addWidget(m_sbPatternWidth, 0, 1);

    detectionLayout->addWidget(new QLabel("内角点高度:", this), 0, 2);
    m_sbPatternHeight = new QSpinBox(this);
    m_sbPatternHeight->setRange(2, 20);
    m_sbPatternHeight->setValue(11);
    detectionLayout->addWidget(m_sbPatternHeight, 0, 3);

    detectionLayout->addWidget(new QLabel("格子大小(mm):", this), 1, 0);
    m_sbSquareSize = new QDoubleSpinBox(this);
    m_sbSquareSize->setRange(1.0, 100.0);
    m_sbSquareSize->setValue(30.0);
    m_sbSquareSize->setDecimals(2);
    detectionLayout->addWidget(m_sbSquareSize, 1, 1);

    m_cbAutoDetect = new QCheckBox("自动检测", this);
    detectionLayout->addWidget(m_cbAutoDetect, 1, 2);

    m_btnDetect = new QPushButton("检测标定板", this);
    m_btnDetect->setEnabled(false);
    connect(m_btnDetect, &QPushButton::clicked, this, &VrEyeViewWidget::onDetectChessboard);
    detectionLayout->addWidget(m_btnDetect, 1, 3);

    mainLayout->addWidget(detectionGroup);

    // 相机内参
    QGroupBox* intrinsicsGroup = new QGroupBox("相机内参", this);
    QGridLayout* intrinsicsLayout = new QGridLayout(intrinsicsGroup);

    intrinsicsLayout->addWidget(new QLabel("fx:", this), 0, 0);
    m_sbFx = new QDoubleSpinBox(this);
    m_sbFx->setRange(100, 5000);
    m_sbFx->setValue(1000.0);
    m_sbFx->setDecimals(2);
    intrinsicsLayout->addWidget(m_sbFx, 0, 1);

    intrinsicsLayout->addWidget(new QLabel("fy:", this), 0, 2);
    m_sbFy = new QDoubleSpinBox(this);
    m_sbFy->setRange(100, 5000);
    m_sbFy->setValue(1000.0);
    m_sbFy->setDecimals(2);
    intrinsicsLayout->addWidget(m_sbFy, 0, 3);

    intrinsicsLayout->addWidget(new QLabel("cx:", this), 1, 0);
    m_sbCx = new QDoubleSpinBox(this);
    m_sbCx->setRange(0, 2000);
    m_sbCx->setValue(640.0);
    m_sbCx->setDecimals(2);
    intrinsicsLayout->addWidget(m_sbCx, 1, 1);

    intrinsicsLayout->addWidget(new QLabel("cy:", this), 1, 2);
    m_sbCy = new QDoubleSpinBox(this);
    m_sbCy->setRange(0, 2000);
    m_sbCy->setValue(480.0);
    m_sbCy->setDecimals(2);
    intrinsicsLayout->addWidget(m_sbCy, 1, 3);

    mainLayout->addWidget(intrinsicsGroup);

    // 检测选项
    QHBoxLayout* optionsLayout = new QHBoxLayout();
    m_cbAdaptiveThresh = new QCheckBox("自适应阈值", this);
    m_cbAdaptiveThresh->setChecked(true);
    m_cbNormalizeImage = new QCheckBox("归一化图像", this);
    m_cbNormalizeImage->setChecked(true);
    optionsLayout->addWidget(m_cbAdaptiveThresh);
    optionsLayout->addWidget(m_cbNormalizeImage);
    optionsLayout->addStretch();
    mainLayout->addLayout(optionsLayout);

    // 状态显示
    m_lblStatus = new QLabel("状态: 未连接", this);
    m_lblDetectionResult = new QLabel("检测结果: 无", this);
    mainLayout->addWidget(m_lblStatus);
    mainLayout->addWidget(m_lblDetectionResult);
}

void VrEyeViewWidget::SetDetectionCallback(DetectionCallback callback)
{
    m_detectionCallback = callback;
}

void VrEyeViewWidget::onConnectCamera()
{
    if (!m_eyeDevice) return;

    QString ip = m_editCameraIP->text();
    int ret = m_eyeDevice->OpenDevice(ip.toStdString().c_str(), false, false, false);

    if (ret == 0) {
        m_isConnected = true;
        m_btnConnect->setEnabled(false);
        m_btnDisconnect->setEnabled(true);
        m_btnStartCapture->setEnabled(true);
        m_lblStatus->setText("状态: 已连接");

        // 设置相机参数
        unsigned int exposure = m_sbExposure->value();
        unsigned int gain = m_sbGain->value();
        m_eyeDevice->SetEyeExpose(exposure);
        m_eyeDevice->SetEyeGain(gain);
    } else {
        QMessageBox::warning(this, "错误", QString("连接相机失败，错误码: %1").arg(ret));
    }
}

void VrEyeViewWidget::onDisconnectCamera()
{
    if (!m_eyeDevice) return;

    if (m_isCapturing) {
        onStopCapture();
    }

    m_eyeDevice->CloseDevice();
    m_isConnected = false;
    m_btnConnect->setEnabled(true);
    m_btnDisconnect->setEnabled(false);
    m_btnStartCapture->setEnabled(false);
    m_lblStatus->setText("状态: 未连接");
}

void VrEyeViewWidget::onStartCapture()
{
    if (!m_eyeDevice || !m_isConnected) return;

    int ret = m_eyeDevice->StartDetect(OnLaserDataCallback, keResultDataType_PointXYZ, this);

    if (ret == 0) {
        m_isCapturing = true;
        m_btnStartCapture->setEnabled(false);
        m_btnStopCapture->setEnabled(true);
        m_btnDetect->setEnabled(true);
        m_lblStatus->setText("状态: 采集中");
    } else {
        QMessageBox::warning(this, "错误", QString("开始采集失败，错误码: %1").arg(ret));
    }
}

void VrEyeViewWidget::onStopCapture()
{
    if (!m_eyeDevice) return;

    m_eyeDevice->StopDetect();
    m_isCapturing = false;
    m_btnStartCapture->setEnabled(true);
    m_btnStopCapture->setEnabled(false);
    m_btnDetect->setEnabled(false);
    m_lblStatus->setText("状态: 已连接");
}

void VrEyeViewWidget::OnLaserDataCallback(EVzResultDataType eDataType,
                                          SVzLaserLineData* pLaserData,
                                          void* pUserData)
{
    VrEyeViewWidget* pThis = static_cast<VrEyeViewWidget*>(pUserData);
    if (pThis) {
        pThis->ProcessLaserData(eDataType, pLaserData);
    }
}

void VrEyeViewWidget::ProcessLaserData(EVzResultDataType eDataType, SVzLaserLineData* pLaserData)
{
    if (!pLaserData) return;
    m_hasNewImage = true;
}

void VrEyeViewWidget::onUpdateDisplay()
{
    // 暂时禁用实时图像显示，VzNLSDK 不支持 RGB
}

void VrEyeViewWidget::onDetectChessboard()
{
    if (!m_detector) return;

    if (m_leftImage.isNull() || m_rightImage.isNull()) {
        m_lblDetectionResult->setText("检测结果: 请先加载左右目图片");
        return;
    }

    // 设置检测参数
    m_detector->SetDetectionFlags(
        m_cbAdaptiveThresh->isChecked(),
        m_cbNormalizeImage->isChecked(),
        false);

    // 准备相机内参
    CameraIntrinsics intrinsics;
    intrinsics.fx = m_sbFx->value();
    intrinsics.fy = m_sbFy->value();
    intrinsics.cx = m_sbCx->value();
    intrinsics.cy = m_sbCy->value();

    // 检测左目标定板
    QImage leftRgb = m_leftImage.convertToFormat(QImage::Format_RGB888);
    ChessboardDetectResult leftResult;
    int ret = m_detector->DetectChessboardWithPose(
        leftRgb.bits(),
        leftRgb.width(),
        leftRgb.height(),
        3,
        m_sbPatternWidth->value(),
        m_sbPatternHeight->value(),
        m_sbSquareSize->value(),
        intrinsics,
        leftResult);

    // 检测右目标定板
    QImage rightRgb = m_rightImage.convertToFormat(QImage::Format_RGB888);
    ChessboardDetectResult rightResult;
    int retRight = m_detector->DetectChessboardWithPose(
        rightRgb.bits(),
        rightRgb.width(),
        rightRgb.height(),
        3,
        m_sbPatternWidth->value(),
        m_sbPatternHeight->value(),
        m_sbSquareSize->value(),
        intrinsics,
        rightResult);

    // 在两张图上绘制角点并显示
    if (ret == 0 && leftResult.detected) {
        m_lastLeftCorners = leftResult.corners;
    } else {
        m_lastLeftCorners.clear();
    }
    if (retRight == 0 && rightResult.detected) {
        m_lastRightCorners = rightResult.corners;
    } else {
        m_lastRightCorners.clear();
    }

    // 在副本上绘制角点并显示（不修改原图）
    QImage leftDisplay = m_leftImage.copy();
    QImage rightDisplay = m_rightImage.copy();
    DrawCorners(leftDisplay, m_lastLeftCorners);
    DrawCorners(rightDisplay, m_lastRightCorners);

    QPixmap leftPm = QPixmap::fromImage(leftDisplay);
    m_leftImageLabel->setPixmap(
        leftPm.scaled(m_leftImageLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
    QPixmap rightPm = QPixmap::fromImage(rightDisplay);
    m_rightImageLabel->setPixmap(
        rightPm.scaled(m_rightImageLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));

    if (ret == 0 && leftResult.detected && retRight == 0 && rightResult.detected) {
        m_lastDetection.detected = true;

        if (leftResult.hasPose) {
            m_lastDetection.x = leftResult.center.x;
            m_lastDetection.y = leftResult.center.y;
            m_lastDetection.z = leftResult.center.z;

            m_lastDetection.nx = leftResult.normal.x;
            m_lastDetection.ny = leftResult.normal.y;
            m_lastDetection.nz = leftResult.normal.z;

            m_lastDetection.rx = leftResult.eulerAngles[0];
            m_lastDetection.ry = leftResult.eulerAngles[1];
            m_lastDetection.rz = leftResult.eulerAngles[2];

            QString resultText = QString("检测结果: "
                                        "左目 %1 个角点, 右目 %2 个角点 | "
                                        "位置: (%.2f, %.2f, %.2f)mm | "
                                        "法向量: (%.3f, %.3f, %.3f) | "
                                        "姿态: (%.2f, %.2f, %.2f)")
                .arg(m_lastLeftCorners.size())
                .arg(m_lastRightCorners.size())
                .arg(m_lastDetection.x).arg(m_lastDetection.y).arg(m_lastDetection.z)
                .arg(m_lastDetection.nx).arg(m_lastDetection.ny).arg(m_lastDetection.nz)
                .arg(m_lastDetection.rx).arg(m_lastDetection.ry).arg(m_lastDetection.rz);
            m_lblDetectionResult->setText(resultText);

            if (m_detectionCallback) {
                m_detectionCallback(m_lastDetection);
            }
            emit chessboardDetected(m_lastDetection);
        }
    } else {
        m_lastDetection.detected = false;

        QString errorMsg = "检测结果: ";
        if (ret != 0 || !leftResult.detected) {
            errorMsg += "左目未检测到标定板 ";
        }
        if (retRight != 0 || !rightResult.detected) {
            errorMsg += "右目未检测到标定板";
        }
        m_lblDetectionResult->setText(errorMsg);
    }
}

void VrEyeViewWidget::onLoadImage()
{
    // 选择一张图片，自动根据 _LeftImg / _RightImg 配对
    QString fileName = QFileDialog::getOpenFileName(
        this,
        "选择左目或右目图片",
        "",
        "图片文件 (*.png *.jpg *.jpeg *.bmp);;所有文件 (*.*)");

    if (fileName.isEmpty()) {
        return;
    }

    QString leftPath, rightPath;

    // 根据文件名中的 _LeftImg / _RightImg 自动匹配配对
    if (fileName.contains("_LeftImg", Qt::CaseInsensitive)) {
        leftPath = fileName;
        rightPath = fileName;
        rightPath.replace("_LeftImg", "_RightImg", Qt::CaseInsensitive);
    } else if (fileName.contains("_RightImg", Qt::CaseInsensitive)) {
        rightPath = fileName;
        leftPath = fileName;
        leftPath.replace("_RightImg", "_LeftImg", Qt::CaseInsensitive);
    } else {
        QMessageBox::warning(this, "错误",
            "文件名中未找到 _LeftImg 或 _RightImg 标识，\n"
            "无法自动配对左右目图像。\n"
            "请选择包含 _LeftImg 或 _RightImg 的文件。");
        return;
    }

    // 加载左目
    QImage leftImage(leftPath);
    if (leftImage.isNull()) {
        QMessageBox::warning(this, "错误",
            QString("无法加载左目图片:\n%1").arg(leftPath));
        return;
    }

    // 加载右目
    QImage rightImage(rightPath);
    if (rightImage.isNull()) {
        QMessageBox::warning(this, "错误",
            QString("无法加载右目图片:\n%1").arg(rightPath));
        return;
    }

    m_leftImage = leftImage;
    m_rightImage = rightImage;

    UpdateImageDisplay();

    QFileInfo fi(leftPath);
    m_lblStatus->setText(QString("状态: 已加载 %1 (%2x%3)")
                         .arg(fi.dir().dirName())
                         .arg(leftImage.width()).arg(leftImage.height()));
    m_btnDetect->setEnabled(true);

    // 自动检测
    if (m_cbAutoDetect->isChecked()) {
        onDetectChessboard();
    }
}

void VrEyeViewWidget::UpdateImageDisplay()
{
    // 左目：保持比例缩放到 label 大小，不拉伸
    if (!m_leftImage.isNull()) {
        QPixmap pm = QPixmap::fromImage(m_leftImage);
        m_leftImageLabel->setPixmap(
            pm.scaled(m_leftImageLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
    }

    // 右目
    if (!m_rightImage.isNull()) {
        QPixmap pm = QPixmap::fromImage(m_rightImage);
        m_rightImageLabel->setPixmap(
            pm.scaled(m_rightImageLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
    }
}

void VrEyeViewWidget::DrawCorners(QImage& image, const std::vector<Point2D>& corners)
{
    if (corners.empty()) return;

    QPainter painter(&image);
    painter.setPen(QPen(Qt::green, 3));

    for (const auto& pt : corners) {
        painter.drawEllipse(QPointF(pt.x, pt.y), 5, 5);
    }
}