GrabBag/GrabBagApp/dialogcameralevel.cpp

#include "dialogcameralevel.h"
#include "ui_dialogcameralevel.h"
#include "VrLog.h"
#include "SG_bagPositioning_Export.h"
#include "PathManager.h"
#include <QThread>
#include <QApplication>
#include <cmath>
#include <mutex>
#include <vector>
#include "StyledMessageBox.h"


#include "LaserDataLoader.h"
#include <QCoreApplication>
#include <QFileInfo>
#include <QTimer>
#include <QFileDialog>
#include <QStandardPaths>

DialogCameraLevel::DialogCameraLevel(QWidget *parent)
    : QDialog(parent)
    , ui(new Ui::DialogCameraLevel)
{
    ui->setupUi(this);

    // 隐藏标题栏
    setWindowFlags(Qt::FramelessWindowHint);
    
    // 初始化界面
    initializeCameraCombo();
    
    // 初始化结果显示区域
    ui->label_level_result->setText("请选择相机并点击调平按钮\n开始相机调平操作");
    ui->label_level_result->setAlignment(Qt::AlignCenter);
}

DialogCameraLevel::~DialogCameraLevel()
{
    // 清理扫描数据缓存
    clearScanDataCache();
    
    // 确保恢复Presenter的状态回调
    restorePresenterStatusCallback();
    
    delete ui;
}

void DialogCameraLevel::setCameraList(const std::vector<IVrEyeDevice*>& cameraList, 
                                     const std::vector<QString>& cameraNames,
                                     GrabBagPresenter* presenter)
{
    m_cameraList = cameraList;
    m_cameraNames = cameraNames;
    m_presenter = presenter;
    
    // 重新初始化相机选择框
    initializeCameraCombo();
}

void DialogCameraLevel::initializeCameraCombo()
{
    ui->combo_camera->clear();
    
    if (m_cameraNames.empty()) {
        ui->combo_camera->setEnabled(false);
        ui->label_level_result->setText("无可用相机设备");
        ui->label_level_result->setAlignment(Qt::AlignCenter);
    } else {
        for (const auto& name : m_cameraNames) {
            ui->combo_camera->addItem(name);
        }
        ui->combo_camera->setEnabled(true);
        
        // 检查并显示当前选中相机的标定状态
        checkAndDisplayCalibrationStatus(0); // 默认选中第一个相机
    }
}

void DialogCameraLevel::on_btn_apply_clicked()
{
    ui->label_level_result->setAlignment(Qt::AlignLeft);
    
#ifndef LEVEL_DEBUG_MODE
    // 检查是否有可用的相机
    if (m_cameraList.empty()) {
        StyledMessageBox::warning(this, "错误", "无可用相机设备！");
        return;
    }
    
    // 获取选中的相机
    int selectedIndex = ui->combo_camera->currentIndex();
    if (selectedIndex < 0 || selectedIndex >= m_cameraList.size()) {
        StyledMessageBox::warning(this, "错误", "请选择有效的相机！");
        return;
    }
#endif
    
    // 清空之前的结果显示
    ui->label_level_result->setText("调平计算中，请稍候...");
    
    // 显示进度提示
    ui->btn_apply->setEnabled(false);
    QApplication::processEvents();
    
    try {
        // 执行相机调平
        if (performCameraLeveling()) {
            // 调平成功，关闭对话框（这会触发析构函数中的回调恢复）
            // accept();
        } else {
            // 显示失败信息到界面
            ui->label_level_result->setText("调平失败！\n\n请检查：\n1. 相机连接是否正常\n2. 地面扫描数据是否充足\n3. 扫描区域是否有足够的地面");
        }
    } catch (const std::exception& e) {
        LOG_ERROR("Camera leveling failed with exception: %s\n", e.what());
        StyledMessageBox::critical(this, "错误", QString("调平过程发生异常：%1").arg(e.what()));
    }
    
    // 恢复按钮状态
    ui->btn_apply->setEnabled(true);
}

void DialogCameraLevel::on_btn_cancel_clicked()
{
    // 直接关闭窗口，不保存任何更改
    reject();
}

bool DialogCameraLevel::performCameraLeveling()
{
    try {
        // 获取选中的相机索引
        int selectedIndex = ui->combo_camera->currentIndex();
        
        // 1. 设置调平状态回调
        setLevelingStatusCallback();
        
#ifdef LEVEL_DEBUG_MODE
        // Debug模式：使用文件对话框选择测试数据
        LOG_INFO("=== DEBUG MODE ENABLED ===\n");
        
        // 选择debug数据文件
        QString debugDataFile = selectDebugDataFile();
        if (debugDataFile.isEmpty()) {
            LOG_INFO("Debug data file selection cancelled\n");
            return false;
        }
        
        // 使用选择的debug数据进行模拟扫描
        if (!loadDebugDataAndSimulateScan(debugDataFile)) {
            LOG_ERROR("Failed to load debug data for camera leveling\n");
            return false;
        }
#else
        // 正常模式：使用真实相机
        if (selectedIndex < 0 || selectedIndex >= m_cameraList.size()) {
            LOG_ERROR("Invalid camera index: %d\n", selectedIndex);
            return false;
        }
        
        LOG_INFO("Performing camera leveling with camera %d (index %d)\n", selectedIndex + 1, selectedIndex);
        
        // 2. 清空之前的扫描数据
        clearScanDataCache();
        
        // 3. 启动相机扫描地面数据
        if (!startCameraScan(selectedIndex)) {
            LOG_ERROR("Failed to start camera scan for leveling\n");
            return false;
        }
        
        // 4. 等待扫描完成（使用状态回调判断）
        LOG_INFO("Collecting ground scan data, waiting for swing finish signal...\n");
        int waitTime = 0;
        const int maxWaitTime = 10000; // 最大等待10秒
        const int checkInterval = 100; // 每100ms检查一次
        
        while (!m_swingFinished && waitTime < maxWaitTime) {
            QThread::msleep(checkInterval);
            QApplication::processEvents(); // 处理UI事件
            waitTime += checkInterval;
        }
        
        // 5. 停止扫描
        stopCameraScan(selectedIndex);
        
        // 检查是否通过状态回调收到了扫描完成信号
        if (m_swingFinished) {
            LOG_INFO("Camera swing finished signal received, scan completed\n");
        } else if (waitTime >= maxWaitTime) {
            LOG_WARNING("Timeout waiting for camera swing finish signal\n");
        }
#endif
        
        // 5. 检查是否收集到足够的数据
        // 6. 调用调平算法计算
        double planeCalib[9];
        double planeHeight;
        double invRMatrix[9];
        
        if (!calculatePlaneCalibration(planeCalib, planeHeight, invRMatrix)) {
            LOG_ERROR("Failed to calculate plane calibration\n");
            return false;
        }
        
        LOG_INFO("Camera leveling calculation completed\n");
        
        // 7. 更新界面显示
        updateLevelingResults(planeCalib, planeHeight, invRMatrix);
        
        // 8. 保存结果到配置
        // 获取相机索引和名称传递给保存方法
        int cameraIndex = selectedIndex + 1;  // 转换为1-based索引
        QString cameraName;
        
#ifdef LEVEL_DEBUG_MODE
        // Debug模式下使用默认名称
        cameraIndex = 1;
        cameraName = QString("Camera_%1").arg(cameraIndex);
#else
        // 正常模式下从列表获取名称
        if (selectedIndex >= 0 && selectedIndex < m_cameraNames.size()) {
            cameraName = m_cameraNames[selectedIndex];
        } else {
            cameraName = QString("Camera_%1").arg(cameraIndex);
        }
#endif
        
        if (!saveLevelingResults(planeCalib, planeHeight, invRMatrix, cameraIndex, cameraName)) {
            LOG_ERROR("Failed to save leveling results\n");
            return false;
        } 
        
        clearScanDataCache();
        LOG_INFO("Camera leveling completed successfully\n");
        
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in performCameraLeveling: %s\n", e.what());
        
        return false;
    }
}

void DialogCameraLevel::updateLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9])
{
    // 计算旋转角度的近似值用于显示
    double rotX = atan2(planeCalib[5], planeCalib[8]) * 180.0 / M_PI;
    double rotY = atan2(-planeCalib[2], sqrt(planeCalib[5]*planeCalib[5] + planeCalib[8]*planeCalib[8])) * 180.0 / M_PI;
    
    // 构建显示文本，将矩阵直接显示到页面上
    QString resultText;
#if 0
    // 基本信息
    resultText += QString("旋转角度 X: %1°\n").arg(QString::number(rotX, 'f', 2));
    resultText += QString("旋转角度 Y: %1°\n").arg(QString::number(rotY, 'f', 2));
#endif

    resultText += QString("地面高度: %1 mm\n").arg(QString::number(planeHeight, 'f', 2));

    // 调平矩阵
    resultText += QString("调平矩阵:\n");
    for (int i = 0; i < 3; i++) {
        resultText += QString("[%1, %2, %3]\n")
            .arg(QString::number(planeCalib[i*3], 'f', 4))
            .arg(QString::number(planeCalib[i*3+1], 'f', 4))
            .arg(QString::number(planeCalib[i*3+2], 'f', 4));
    }
    
    resultText += QString("逆旋转矩阵:\n");
    for (int i = 0; i < 3; i++) {
        resultText += QString("[%1, %2, %3]\n")
            .arg(QString::number(invRMatrix[i*3], 'f', 4))
            .arg(QString::number(invRMatrix[i*3+1], 'f', 4))
            .arg(QString::number(invRMatrix[i*3+2], 'f', 4));
    }

    // 将结果显示到界面上
    ui->label_level_result->setText(resultText);
    ui->label_level_result->setAlignment(Qt::AlignLeft | Qt::AlignTop);
}

// 启动相机扫描
bool DialogCameraLevel::startCameraScan(int cameraIndex)
{
    if (cameraIndex < 0 || cameraIndex >= m_cameraList.size()) {
        LOG_ERROR("Invalid camera index for scan: %d\n", cameraIndex);
        return false;
    }
    
    IVrEyeDevice* camera = m_cameraList[cameraIndex];
    if (!camera) {
        LOG_ERROR("Camera device is null at index: %d\n", cameraIndex);
        return false;
    }
    
    // 启动相机检测，使用静态回调函数
    int result = camera->StartDetect(&DialogCameraLevel::StaticDetectionCallback, this);
    if (result != 0) {
        LOG_ERROR("Failed to start camera detection: %d\n", result);
        return false;
    }
    
    LOG_INFO("Camera scan started successfully for camera index: %d\n", cameraIndex);
    return true;
}

// 停止相机扫描
bool DialogCameraLevel::stopCameraScan(int cameraIndex)
{
    if (cameraIndex < 0 || cameraIndex >= m_cameraList.size()) {
        LOG_ERROR("Invalid camera index for stop scan: %d\n", cameraIndex);
        return false;
    }
    
    IVrEyeDevice* camera = m_cameraList[cameraIndex];
    if (!camera) {
        LOG_ERROR("Camera device is null at index: %d\n", cameraIndex);
        return false;
    }
    
    int result = camera->StopDetect();
    if (result != 0) {
        LOG_WARNING("Failed to stop camera detection, error: %d\n", result);
        return false;
    }
    
    LOG_INFO("Camera scan stopped successfully for camera index: %d\n", cameraIndex);
    return true;
}

// 静态检测回调函数
void DialogCameraLevel::StaticDetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pUserData)
{
    DialogCameraLevel* pThis = reinterpret_cast<DialogCameraLevel*>(pUserData);
    if (pThis && pLaserLinePoint) {
        pThis->DetectionCallback(eDataType, pLaserLinePoint);
    }
}

// 静态状态回调函数
void DialogCameraLevel::StaticStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
    DialogCameraLevel* pThis = reinterpret_cast<DialogCameraLevel*>(pInfoParam);
    if (pThis) {
        pThis->StatusCallback(eStatus, pExtData, nDataLength, pInfoParam);
    }
}

// 状态回调函数实例版本
void DialogCameraLevel::StatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
    LOG_DEBUG("[Leveling Status Callback] received: status=%d\n", (int)eStatus);
    
    switch (eStatus) {
        case EVzDeviceWorkStatus::keDeviceWorkStatus_Device_Swing_Finish:
        {
            LOG_INFO("[Leveling Status Callback] Camera swing finished, scan completed\n");
            m_swingFinished = true;  // 摆动完成即表示扫描完成
            break;
        }
        default:
            LOG_DEBUG("[Leveling Status Callback] Other status: %d\n", (int)eStatus);
            break;
    }
}

// 检测数据回调函数实例版本
void DialogCameraLevel::DetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint)
{
    if (!pLaserLinePoint) {
        LOG_WARNING("[Leveling Callback] pLaserLinePoint is null\n");
        return;
    }

    if (pLaserLinePoint->nPointCount <= 0) {
        LOG_WARNING("[Leveling Callback] Point count is zero or negative: %d\n", pLaserLinePoint->nPointCount);
        return;
    }

    if (!pLaserLinePoint->p3DPoint) {
        LOG_WARNING("[Leveling Callback] p3DPoint is null\n");
        return;
    }

    // 转换数据格式：从SVzLaserLineData转换为SVzNL3DLaserLine并存储到缓存
    SVzNL3DLaserLine laser3DLine;
    
    // 复制基本信息
    laser3DLine.nTimeStamp = pLaserLinePoint->llTimeStamp;
    laser3DLine.nPositionCnt = pLaserLinePoint->nPointCount;

    // 分配和复制点云数据
    laser3DLine.p3DPosition = new SVzNL3DPosition[pLaserLinePoint->nPointCount];
    // 复制点云数据
    memcpy(laser3DLine.p3DPosition, pLaserLinePoint->p3DPoint, sizeof(SVzNL3DPosition) * pLaserLinePoint->nPointCount);
    
    // 将转换后的数据保存到缓存中
    std::lock_guard<std::mutex> lock(m_scanDataMutex);
    m_scanDataCache.push_back(laser3DLine);
}

// 调平算法计算
bool DialogCameraLevel::calculatePlaneCalibration(double planeCalib[9], double& planeHeight, double invRMatrix[9])
{
    std::lock_guard<std::mutex> lock(m_scanDataMutex);
    // 检查是否有足够的扫描数据
    if (m_scanDataCache.empty()) {
        LOG_ERROR("No scan data available for plane calibration\n");
        return false;
    }
    
    LOG_INFO("Calculating plane calibration from %zu scan lines\n", m_scanDataCache.size());
    
    try {
        // 调用实际的调平算法
        SSG_planeCalibPara calibResult = sg_getBagBaseCalibPara(m_scanDataCache.data(), static_cast<int>(m_scanDataCache.size()));
        
        // 将结构体中的数据复制到输出参数
        for (int i = 0; i < 9; i++) {
            planeCalib[i] = calibResult.planeCalib[i];
            invRMatrix[i] = calibResult.invRMatrix[i];
        }
        planeHeight = calibResult.planeHeight;
        
        // 计算旋转角度用于日志显示
        double rotAngleX = atan2(planeCalib[5], planeCalib[8]) * 180.0 / M_PI;
        double rotAngleY = atan2(-planeCalib[2], sqrt(planeCalib[5]*planeCalib[5] + planeCalib[8]*planeCalib[8])) * 180.0 / M_PI;
        
        LOG_INFO("Plane calibration calculated: height=%.3f, rotX=%.2f°, rotY=%.2f°\n", 
                 planeHeight, rotAngleX, rotAngleY);
        
#ifdef LEVEL_DEBUG_MODE
        // 统计点云数据
        int totalPoints = 0;
        double avgHeight = 0.0;
        for (const auto& scanLine : m_scanDataCache) {
            for (int i = 0; i < scanLine.nPositionCnt; i++) {
                if (scanLine.p3DPosition[i].pt3D.z > -1000 && scanLine.p3DPosition[i].pt3D.z < 1000) {
                    avgHeight += scanLine.p3DPosition[i].pt3D.z;
                    totalPoints++;
                }
            }
        }
        
        LOG_INFO("=== DEBUG MODE CALIBRATION RESULTS ===\n");

        LOG_INFO("Algorithm results:\n");
        LOG_INFO("  Calculated plane height: %.3f mm\n", calibResult.planeHeight);
        LOG_INFO("Calibration matrix:\n");
        for (int i = 0; i < 3; i++) {
            LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[i*3], calibResult.planeCalib[i*3+1], calibResult.planeCalib[i*3+2]);
        }
        LOG_INFO("Inverse rotation matrix:\n");
        for (int i = 0; i < 3; i++) {
            LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[i*3], calibResult.invRMatrix[i*3+1], calibResult.invRMatrix[i*3+2]);
        }
        LOG_INFO("=======================================\n");
#endif
        
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in sg_getBagBaseCalibPara: %s\n", e.what());
        return false;
    } catch (...) {
        LOG_ERROR("Unknown exception in sg_getBagBaseCalibPara\n");
        return false;
    }
}

// 清空扫描数据缓存
void DialogCameraLevel::clearScanDataCache()
{
    std::lock_guard<std::mutex> lock(m_scanDataMutex);
    
    LOG_DEBUG("Clearing scan data cache, current size: %zu\n", m_scanDataCache.size());
    
    // 释放缓存的内存
    for (auto& cachedLine : m_scanDataCache) {
        if (cachedLine.p3DPosition) {
            delete[] cachedLine.p3DPosition;
            cachedLine.p3DPosition = nullptr;
        }
    }
    
    // 清空缓存容器
    m_scanDataCache.clear();
    
    LOG_DEBUG("Scan data cache cleared successfully\n");
}

bool DialogCameraLevel::saveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9], int cameraIndex, const QString& cameraName)
{
    try {
        if (!m_presenter) {
            LOG_ERROR("Presenter is null, cannot save leveling results\n");
            return false;
        }
        
        // 获取配置对象
        IVrConfig* config = m_presenter->GetConfig();
        if (!config) {
            LOG_ERROR("Config is null, cannot save leveling results\n");
            return false;
        }
        
        // 验证传入的相机参数
        if (cameraIndex <= 0) {
            LOG_ERROR("Invalid camera index: %d\n", cameraIndex);
            return false;
        }
        
        if (cameraName.isEmpty()) {
            LOG_ERROR("Camera name is empty\n");
            return false;
        }
        
        // 加载当前配置
        QString configPath = PathManager::GetConfigFilePath();
        LOG_INFO("Config path: %s\n", configPath.toUtf8().constData());
        ConfigResult configResult = config->LoadConfig(configPath.toStdString());
        
        // 创建或更新指定相机的调平参数
        VrCameraPlaneCalibParam cameraParam;
        cameraParam.cameraIndex = cameraIndex;
        cameraParam.cameraName = cameraName.toStdString();
        cameraParam.planeHeight = planeHeight;
        
        // 复制校准矩阵
        for (int i = 0; i < 9; i++) {
            cameraParam.planeCalib[i] = planeCalib[i];
            cameraParam.invRMatrix[i] = invRMatrix[i];
        }
        
        // 更新配置中的相机校准参数
        configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(cameraParam);
        
        // 保存配置
        bool saveResult = config->SaveConfig(configPath.toStdString(), configResult);
        if (!saveResult) {
            LOG_ERROR("Failed to save config with leveling results\n");
            return false;
        }
        
        LOG_INFO("Leveling results saved successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
        LOG_INFO("Plane height: %.3f\n", planeHeight);
        LOG_INFO("Calibration marked as completed\n");
        
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in saveLevelingResults: %s\n", e.what());
        return false;
    }
}

// 选择debug数据文件
QString DialogCameraLevel::selectDebugDataFile()
{
    // 获取应用程序目录作为起始目录
    QString appDir = QCoreApplication::applicationDirPath();
    QString testDataDir = appDir + "/TestData";
    
    // 如果TestData目录不存在，使用应用程序目录
    if (!QFileInfo::exists(testDataDir)) {
        testDataDir = appDir;
    }
    
    // 打开文件选择对话框
    QString fileName = QFileDialog::getOpenFileName(
        this,
        "选择激光扫描数据文件",
        testDataDir,
        "文本文件 (*.txt);;所有文件 (*.*)"
    );
    
    if (fileName.isEmpty()) {
        LOG_INFO("No debug data file selected\n");
        return "";
    }
    
    // 检查文件是否存在
    if (!QFileInfo::exists(fileName)) {
        LOG_ERROR("Selected debug data file does not exist: %s\n", fileName.toUtf8().constData());
        StyledMessageBox::warning(this, "文件错误", "选择的文件不存在！");
        return "";
    }
    
    LOG_INFO("Selected debug data file: %s\n", fileName.toUtf8().constData());
    
    return fileName;
}

// 加载debug数据并模拟扫描过程
bool DialogCameraLevel::loadDebugDataAndSimulateScan(const QString& filePath)
{
    LOG_INFO("Loading debug data from: %s\n", filePath.toUtf8().constData());
    
    try {
        // 清空之前的扫描数据
        clearScanDataCache();
        
        // 使用LaserDataLoader加载测试数据
        LaserDataLoader dataLoader;
        std::vector<SVzNL3DLaserLine> debugData;
        int lineNum = 0;
        float scanSpeed = 0.0f;
        int maxTimeStamp = 0;
        int clockPerSecond = 0;
        
        int result = dataLoader.LoadLaserScanData(filePath.toStdString(), debugData, 
                                                 lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
        
        if (result != 0) {
            LOG_ERROR("Failed to load debug data: %s\n", dataLoader.GetLastError().c_str());
            StyledMessageBox::critical(this, "Debug模式错误", 
                                     QString("加载测试数据失败：\n%1").arg(QString::fromStdString(dataLoader.GetLastError())));
            return false;
        }
        
        if (debugData.empty()) {
            LOG_ERROR("Debug data is empty\n");
            StyledMessageBox::warning(this, "Debug模式警告", "测试数据文件为空！");
            return false;
        }
        
        LOG_INFO("Loaded %d lines of debug data, starting simulation\n", lineNum);
        
        // 将加载的数据复制到缓存中（需要深拷贝）
        {
            std::lock_guard<std::mutex> lock(m_scanDataMutex);
            m_scanDataCache.reserve(debugData.size());
            
            for (const auto& line : debugData) {
                SVzNL3DLaserLine copyLine;
                copyLine.nTimeStamp = line.nTimeStamp;
                copyLine.nPositionCnt = line.nPositionCnt;
                
                if (line.p3DPosition && line.nPositionCnt > 0) {
                    copyLine.p3DPosition = new SVzNL3DPosition[line.nPositionCnt];
                    memcpy(copyLine.p3DPosition, line.p3DPosition, sizeof(SVzNL3DPosition) * line.nPositionCnt);
                } else {
                    copyLine.p3DPosition = nullptr;
                }
                
                m_scanDataCache.push_back(copyLine);
            }
        }
        
        // 释放临时加载的数据
        dataLoader.FreeLaserScanData(debugData);
        
        // 启动模拟扫描过程
        simulateScanProcess();
        
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in loadDebugDataAndSimulateScan: %s\n", e.what());
        StyledMessageBox::critical(this, "Debug模式异常",  QString("加载debug数据时发生异常：\n%1").arg(e.what()));
        return false;
    }
}

// 模拟扫描过程
void DialogCameraLevel::simulateScanProcess()
{
    LOG_INFO("Starting scan simulation process\n");
    
    // 创建定时器来模拟数据收集过程
    QTimer* simulationTimer = new QTimer();
    simulationTimer->setSingleShot(true);
    
    // 模拟扫描需要的时间（2秒）
    simulationTimer->setInterval(2000);
    
    // 连接定时器信号
    connect(simulationTimer, &QTimer::timeout, [this, simulationTimer]() {
        LOG_INFO("Scan simulation completed\n");
        m_swingFinished = true;  // Debug模式下模拟摆动完成
        
        // 显示调试信息
        {
            std::lock_guard<std::mutex> lock(m_scanDataMutex);
            LOG_INFO("Debug scan simulation completed with %zu lines\n", m_scanDataCache.size());
            
            // 统计点云数据
            int totalPoints = 0;
            for (const auto& line : m_scanDataCache) {
                totalPoints += line.nPositionCnt;
            }
            LOG_INFO("Total points in debug data: %d\n", totalPoints);
        }
        
        // 清理定时器
        simulationTimer->deleteLater();
    });
    
    // 启动模拟扫描
    simulationTimer->start();
    
    LOG_INFO("Simulation timer started, waiting for completion...\n");
}

// 相机选择改变的槽函数
void DialogCameraLevel::on_combo_camera_currentIndexChanged(int index)
{
    if (index >= 0 && index < m_cameraNames.size()) {
        LOG_INFO("Camera selection changed to index: %d (%s)\n", index, m_cameraNames[index].toUtf8().constData());
        checkAndDisplayCalibrationStatus(index);
    } else {
        LOG_WARNING("Invalid camera index selected: %d\n", index);
        ui->label_level_result->setText("无效的相机选择");
        ui->label_level_result->setAlignment(Qt::AlignCenter);
    }
}

// 加载相机标定数据
bool DialogCameraLevel::loadCameraCalibrationData(int cameraIndex, const QString& cameraName, 
                                                  double planeCalib[9], double& planeHeight, double invRMatrix[9])
{
    try {
        if (!m_presenter) {
            LOG_ERROR("Presenter is null, cannot load calibration data\n");
            return false;
        }
        
        // 获取配置对象
        IVrConfig* config = m_presenter->GetConfig();
        if (!config) {
            LOG_ERROR("Config is null, cannot load calibration data\n");
            return false;
        }
        
        // 加载配置文件
        QString configPath = PathManager::GetConfigFilePath();
        ConfigResult configResult = config->LoadConfig(configPath.toStdString());
        
        // 获取指定相机的标定参数
        const VrCameraPlaneCalibParam* cameraParam = configResult.algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex);
        
        if (!cameraParam || !cameraParam->isCalibrated) {
            LOG_INFO("No calibration data found for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
            return false;
        }
        
        // 复制标定数据
        for (int i = 0; i < 9; i++) {
            planeCalib[i] = cameraParam->planeCalib[i];
            invRMatrix[i] = cameraParam->invRMatrix[i];
        }
        planeHeight = cameraParam->planeHeight;
        
        LOG_INFO("Calibration data loaded successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
        LOG_INFO("Plane height: %.3f\n", planeHeight);
        
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in loadCameraCalibrationData: %s\n", e.what());
        return false;
    }
}

// 检查并显示相机标定状态
void DialogCameraLevel::checkAndDisplayCalibrationStatus(int cameraIndex)
{
    if (cameraIndex < 0 || cameraIndex >= m_cameraNames.size()) {
        LOG_WARNING("Invalid camera index for status check: %d\n", cameraIndex);
        ui->label_level_result->setText("无效的相机索引");
        ui->label_level_result->setAlignment(Qt::AlignCenter);
        return;
    }
    
    QString cameraName = m_cameraNames[cameraIndex];
    int configCameraIndex = cameraIndex + 1; // 转换为1-based索引
    
    // 尝试加载该相机的标定数据
    double planeCalib[9];
    double planeHeight;
    double invRMatrix[9];
    
    if (loadCameraCalibrationData(configCameraIndex, cameraName, planeCalib, planeHeight, invRMatrix)) {
        // 有标定数据，显示标定结果
        LOG_INFO("Displaying existing calibration data for camera %s\n", cameraName.toUtf8().constData());
        updateLevelingResults(planeCalib, planeHeight, invRMatrix);
    } else {
        // 没有标定数据，显示提示信息
        LOG_INFO("No calibration data found for camera %s, showing instruction\n", cameraName.toUtf8().constData());
        ui->label_level_result->setText(QString("请选择相机 \"%1\" 并点击调平按钮\n开始相机调平操作").arg(cameraName));
        ui->label_level_result->setAlignment(Qt::AlignCenter);
    }
}

// 设置调平时的状态回调
void DialogCameraLevel::setLevelingStatusCallback()
{
    if (!m_presenter) {
        LOG_ERROR("Presenter is null, cannot set leveling status callback\n");
        return;
    }
    
    // 为所有相机设置调平状态回调
    m_presenter->SetCameraStatusCallback(&DialogCameraLevel::StaticStatusCallback, this);
    
    // 重置状态标志
    m_swingFinished = false;
    m_callbackRestored = false;  // 重置恢复标志，允许稍后恢复
    
    LOG_INFO("Leveling status callback set for all cameras\n");
}

// 恢复Presenter的状态回调
void DialogCameraLevel::restorePresenterStatusCallback()
{
    // 检查是否已经恢复过回调，避免重复调用
    if (m_callbackRestored.exchange(true)) {
        LOG_DEBUG("Presenter status callback already restored, skipping\n");
        return;
    }
    
    if (!m_presenter) {
        LOG_ERROR("Presenter is null, cannot restore status callback\n");
        return;
    }
    
    // 恢复Presenter的状态回调 - 使用GrabBagPresenter的静态回调
    m_presenter->SetCameraStatusCallback(&GrabBagPresenter::_StaticCameraNotify, m_presenter);
    
    LOG_INFO("Presenter status callback restored for all cameras\n");
}