#include "RodAndBarPositionPresenter.h"
#include "VrError.h"
#include "VrLog.h"
#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QDir>
#include <QtCore/QString>
#include <QtCore/QStandardPaths>
#include <QtCore/QFile>
#include <QtCore/QDateTime>
#include <cmath>
#include <algorithm>
#include <QImage>
#include <QThread>
#include <atomic>
#include <cstring>

#include "Version.h"
#include "VrTimeUtils.h"
#include "VrDateUtils.h"
#include "SG_baseDataType.h"
#include "VrConvert.h"
#include "DetectPresenter.h"
#include "PathManager.h"

RodAndBarPositionPresenter::RodAndBarPositionPresenter(QObject *parent)
    : BasePresenter(parent)
    , m_pConfigManager(nullptr)
    , m_pDetectPresenter(nullptr)
{
    // 基类已经创建了相机重连定时器和检测数据缓存
}

RodAndBarPositionPresenter::~RodAndBarPositionPresenter()
{
    // 基类会自动处理：相机重连定时器、算法检测线程、检测数据缓存、相机设备资源

    // 释放ConfigManager（析构函数会自动调用Shutdown）
    if (m_pConfigManager) {
        delete m_pConfigManager;
        m_pConfigManager = nullptr;
    }

    // 释放检测处理器
    if(m_pDetectPresenter)
    {
        delete m_pDetectPresenter;
        m_pDetectPresenter = nullptr;
    }
}

int RodAndBarPositionPresenter::InitApp()
{
    LOG_DEBUG("Start APP Version: %s\n", RODANDBARPOSITION_FULL_VERSION_STRING);

    // 初始化连接状态
    SetWorkStatus(WorkStatus::InitIng);

    m_pDetectPresenter = new DetectPresenter();

    int nRet = SUCCESS;

    // 创建 ConfigManager 实例
    m_pConfigManager = new ConfigManager();
    if (!m_pConfigManager) {
        LOG_ERROR("Failed to create ConfigManager instance\n");
        if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate("配置管理器创建失败");
        return ERR_CODE(DEV_CONFIG_ERR);
    }

    // 初始化 ConfigManager
    if (!m_pConfigManager->Initialize()) {
        LOG_ERROR("Failed to initialize ConfigManager\n");
        if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate("配置管理器初始化失败");
        return ERR_CODE(DEV_CONFIG_ERR);
    }

    LOG_INFO("Configuration loaded successfully\n");

    // 获取配置结果
    ConfigResult configResult = m_pConfigManager->GetConfigResult();

    // 调用基类InitCamera进行相机初始化（bRGB=false, bSwing=true）
    InitCamera(configResult.cameraList, false, true);

    LOG_INFO("Camera initialization completed. Connected cameras: %zu, default camera index: %d\n",
             m_vrEyeDeviceList.size(), m_currentCameraIndex);

    if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate("设备初始化完成");

    CheckAndUpdateWorkStatus();

    if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate("配置初始化成功");

    return SUCCESS;
}

// 初始化算法参数（实现BasePresenter纯虚函数）
int RodAndBarPositionPresenter::InitAlgoParams()
{
    LOG_DEBUG("initializing algorithm parameters\n");
    QString exePath = QCoreApplication::applicationFilePath();

    // 清空现有的手眼标定矩阵列表
    m_clibMatrixList.clear();

    // 从 ConfigManager 获取配置结果
    ConfigResult configResult = m_pConfigManager->GetConfigResult();

    // 从 config.xml 加载手眼标定矩阵列表（支持多相机）
    for (const auto& handEyeMatrix : configResult.handEyeCalibMatrixList) {
        CalibMatrix calibMatrix;
        memcpy(calibMatrix.clibMatrix, handEyeMatrix.matrix, sizeof(double) * 16);
        m_clibMatrixList.push_back(calibMatrix);

        LOG_INFO("Loaded hand-eye calibration matrix for camera %d:\n", handEyeMatrix.cameraIndex);
        QString clibMatrixStr;
        for (int i = 0; i < 4; ++i) {
            clibMatrixStr.clear();
            for (int j = 0; j < 4; ++j) {
                clibMatrixStr += QString::asprintf("%8.4f ", calibMatrix.clibMatrix[i * 4 + j]);
            }
            LOG_INFO("  %s\n", clibMatrixStr.toStdString().c_str());
        }
    }

    LOG_INFO("Total loaded %zu hand-eye calibration matrices\n", m_clibMatrixList.size());

    const VrAlgorithmParams& xmlParams = configResult.algorithmParams;

    LOG_INFO("Loaded XML params - Rod: rodDiameter=%.1f, rodLen=%.1f\n",
             xmlParams.rodParam.rodDiameter,
             xmlParams.rodParam.rodLen);
    LOG_INFO("Loaded XML params - Filter: continuityTh=%.1f, outlierTh=%.1f\n",
             xmlParams.filterParam.continuityTh, xmlParams.filterParam.outlierTh);

    LOG_INFO("Algorithm parameters initialized successfully\n");

    return SUCCESS;
}

// 获取算法版本
QString RodAndBarPositionPresenter::GetAlgoVersion() const
{
    return DetectPresenter::GetAlgoVersion();
}

// 手眼标定矩阵管理方法实现
CalibMatrix RodAndBarPositionPresenter::GetClibMatrix(int index) const
{
    CalibMatrix clibMatrix;

    double initClibMatrix[16] = {
        1.0, 0.0, 0.0, 0.0,  // 第一行
        0.0, 1.0, 0.0, 0.0,  // 第二行
        0.0, 0.0, 1.0, 0.0,  // 第三行
        0.0, 0.0, 0.0, 1.0   // 第四行
    };
    memcpy(clibMatrix.clibMatrix, initClibMatrix, sizeof(initClibMatrix));

    if (index >= 0 && index < static_cast<int>(m_clibMatrixList.size())) {
        clibMatrix = m_clibMatrixList[index];
        memcpy(clibMatrix.clibMatrix, m_clibMatrixList[index].clibMatrix, sizeof(initClibMatrix));
    } else {
        LOG_WARNING("Invalid hand-eye calibration matrix\n");
    }

    return clibMatrix;
}


void RodAndBarPositionPresenter::CheckAndUpdateWorkStatus()
{
    if (m_bCameraConnected) {
        SetWorkStatus(WorkStatus::Ready);
    } else {
        SetWorkStatus(WorkStatus::Error);
    }
}

// 实现BasePresenter纯虚函数：执行算法检测
int RodAndBarPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
{
    LOG_INFO("[Algo Thread] Start real detection task using algorithm\n");

    // 1. 获取缓存的点云数据（已由基类验证非空）
    unsigned int lineNum = detectionDataCache.size();
    if(GetStatusCallback<IYRodAndBarPositionStatus>()){
        if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate("扫描线数:" + std::to_string(lineNum) + "，正在算法检测...");
    }

    // 检查检测处理器是否已初始化
    if (!m_pDetectPresenter) {
        LOG_ERROR("DetectPresenter is null, cannot proceed with detection\n");
        if (GetStatusCallback<IYRodAndBarPositionStatus>()) {
            if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate("检测处理器未初始化");
        }
        return ERR_CODE(DEV_NOT_FIND);
    }

    CVrTimeUtils oTimeUtils;

    // 获取当前使用的手眼标定矩阵
    const CalibMatrix currentClibMatrix = GetClibMatrix(m_currentCameraIndex - 1);

    // 从 ConfigManager 获取算法参数和调试参数
    VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
    ConfigResult configResult = m_pConfigManager->GetConfigResult();
    VrDebugParam debugParam = configResult.debugParam;
    int eulerOrder = configResult.eulerOrder;
    const int calibIdx = m_currentCameraIndex - 1;
    if (calibIdx >= 0 && calibIdx < static_cast<int>(configResult.handEyeCalibMatrixList.size())) {
        eulerOrder = configResult.handEyeCalibMatrixList[calibIdx].eulerOrder;
    } else if (!configResult.handEyeCalibMatrixList.empty()) {
        eulerOrder = configResult.handEyeCalibMatrixList[0].eulerOrder;
    }
    const int dirVectorInvert = configResult.dirVectorInvert;
    LOG_INFO("[Algo Thread] Using euler order: %d, dir vector invert: %d\n", eulerOrder, dirVectorInvert);

    DetectionResult detectionResult;

    int nRet = m_pDetectPresenter->DetectRod(m_currentCameraIndex, detectionDataCache,
                                algorithmParams, debugParam, m_dataLoader,
                                currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert,
                                detectionResult);
    // 根据项目类型选择处理方式
    if (GetStatusCallback<IYRodAndBarPositionStatus>()) {
        QString err = QString("错误:%1").arg(nRet);
        if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate(QString("检测%1").arg(SUCCESS == nRet ? "成功": err).toStdString());
    }

    LOG_INFO("[Algo Thread] sx_rodPositioning detected %zu objects time : %.2f ms\n", detectionResult.positions.size(), oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(nRet);

    // 8. 通知UI检测结果
    detectionResult.cameraIndex = m_currentCameraIndex;
    if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) {
        pStatus->OnDetectionResult(detectionResult);
    }

    // 更新状态
    QString statusMsg = QString("检测完成，发现%1个棒材").arg(detectionResult.positions.size());
    if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());

    // 缓存检测结果，重置索引（首次发送index=0，下次写10从1开始）
    {
        std::lock_guard<std::mutex> lock(m_resultCacheMutex);
        m_lastDetectionResult = detectionResult;
        m_resultIndex = 1;
    }
    // 发送第0个结果
    _SendDetectionResultToModbus(detectionResult, m_currentCameraIndex, 0);

    // 9. 检测完成后，将工作状态更新为"完成"
    SetWorkStatus(WorkStatus::Completed);

    // 恢复到就绪状态
    SetWorkStatus(WorkStatus::Ready);

    return SUCCESS;
}


// 实现配置改变通知接口
void RodAndBarPositionPresenter::OnConfigChanged(const ConfigResult& configResult)
{
    LOG_INFO("Configuration changed notification received, reloading algorithm parameters\n");

    // 重新初始化算法参数
    int result = InitAlgoParams();
    if (result == SUCCESS) {
        LOG_INFO("Algorithm parameters reloaded successfully after config change\n");
        if (GetStatusCallback<IYRodAndBarPositionStatus>()) {
            if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate("配置已更新，算法参数重新加载成功");
        }
    } else {
        LOG_ERROR("Failed to reload algorithm parameters after config change, error: %d\n", result);
        if (GetStatusCallback<IYRodAndBarPositionStatus>()) {
            if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate("配置更新后算法参数重新加载失败");
        }
    }
}

// 根据相机索引获取调平参数
SSG_planeCalibPara RodAndBarPositionPresenter::_GetCameraCalibParam(int cameraIndex)
{
    // 查找指定相机索引的调平参数
    SSG_planeCalibPara calibParam;

    // 使用单位矩阵（未校准状态）
    double identityMatrix[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
    for (int i = 0; i < 9; i++) {
        calibParam.planeCalib[i] = identityMatrix[i];
        calibParam.invRMatrix[i] = identityMatrix[i];
    }
    calibParam.planeHeight = -1.0;  // 使用默认高度

    // 从 ConfigManager 获取算法参数
    VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();

    for (const auto& cameraParam : algorithmParams.planeCalibParam.cameraCalibParams) {
        if (cameraParam.cameraIndex == cameraIndex) {

            // 根据isCalibrated标志决定使用标定矩阵还是单位矩阵
            if (cameraParam.isCalibrated) {
                // 使用实际的标定矩阵
                for (int i = 0; i < 9; i++) {
                    calibParam.planeCalib[i] = cameraParam.planeCalib[i];
                    calibParam.invRMatrix[i] = cameraParam.invRMatrix[i];
                }
                calibParam.planeHeight = cameraParam.planeHeight;
            }
        }
    }
    return calibParam;
}

// 实现BasePresenter纯虚函数：相机状态变化通知
void RodAndBarPositionPresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
{
    LOG_INFO("Camera %d status changed: %s\n", cameraIndex, isConnected ? "connected" : "disconnected");

    // 通知UI更新相机状态
    if (GetStatusCallback<IYRodAndBarPositionStatus>()) {
        if (cameraIndex == 1) {
            if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnCamera1StatusChanged(isConnected);
        } else if (cameraIndex == 2) {
            if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnCamera2StatusChanged(isConnected);
        }

        // 获取相机名称用于状态消息
        QString cameraName;
        int arrayIndex = cameraIndex - 1;
        if (arrayIndex >= 0 && arrayIndex < static_cast<int>(m_vrEyeDeviceList.size())) {
            cameraName = QString::fromStdString(m_vrEyeDeviceList[arrayIndex].first);
        } else {
            cameraName = QString("相机%1").arg(cameraIndex);
        }

        QString statusMsg = QString("%1%2").arg(cameraName).arg(isConnected ? "已连接" : "已断开");
        if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());
    }

    // 检查并更新工作状态
    CheckAndUpdateWorkStatus();
}

// 实现BasePresenter虚函数：工作状态变化通知
void RodAndBarPositionPresenter::OnWorkStatusChanged(WorkStatus status)
{
    auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>();
    if (pStatus) {
        pStatus->OnWorkStatusChanged(status);
    }
}


// 实现BasePresenter虚函数：相机数量变化通知
void RodAndBarPositionPresenter::OnCameraCountChanged(int count)
{
    auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>();
    if (pStatus) {
        pStatus->OnCameraCountChanged(count);
    }
}

// 实现BasePresenter虚函数：状态文字更新通知
void RodAndBarPositionPresenter::OnStatusUpdate(const std::string& statusMessage)
{
    auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>();
    if (pStatus) {
        pStatus->OnStatusUpdate(statusMessage);
    }
}

// ============ 实现 ICameraLevelCalculator 接口 ============

bool RodAndBarPositionPresenter::CalculatePlaneCalibration(
    const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData,
    double planeCalib[9],
    double& planeHeight,
    double invRMatrix[9])
{
    try {
        // 检查是否有足够的扫描数据
        if (scanData.empty()) {
            LOG_ERROR("No scan data available for plane calibration\n");
            return false;
        }

        LOG_INFO("Calculating plane calibration from %zu scan lines\n", scanData.size());

        // 转换为算法需要的XYZ格式
        LaserDataLoader dataLoader;
        std::vector<std::vector<SVzNL3DPosition>> xyzData;
        int convertResult = dataLoader.ConvertToSVzNL3DPosition(scanData, xyzData);
        if (convertResult != SUCCESS || xyzData.empty()) {
            LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
            return false;
        }

        // 棒材定位项目暂时使用简单的平面拟合
        // 使用默认的单位矩阵
        double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        memcpy(planeCalib, identity, sizeof(double) * 9);
        memcpy(invRMatrix, identity, sizeof(double) * 9);
        planeHeight = -1.0;

        LOG_INFO("Plane calibration calculated successfully: height=%.3f\n", planeHeight);
        return true;

    } catch (const std::exception& e) {
        LOG_ERROR("Exception in CalculatePlaneCalibration: %s\n", e.what());
        return false;
    } catch (...) {
        LOG_ERROR("Unknown exception in CalculatePlaneCalibration\n");
        return false;
    }
}

// ============ 实现 ICameraLevelResultSaver 接口 ============

bool RodAndBarPositionPresenter::SaveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9],
                                              int cameraIndex, const QString& cameraName)
{
    try {
        if (!m_pConfigManager) {
            LOG_ERROR("ConfigManager 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::GetInstance().GetConfigFilePath();
        LOG_INFO("Config path: %s\n", configPath.toUtf8().constData());

        SystemConfig systemConfig = m_pConfigManager->GetConfig();

        // 创建或更新指定相机的调平参数
        VrCameraPlaneCalibParam cameraParam;
        cameraParam.cameraIndex = cameraIndex;
        cameraParam.cameraName = cameraName.toStdString();
        cameraParam.planeHeight = planeHeight;
        cameraParam.isCalibrated = true;

        // 复制校准矩阵
        for (int i = 0; i < 9; i++) {
            cameraParam.planeCalib[i] = planeCalib[i];
            cameraParam.invRMatrix[i] = invRMatrix[i];
        }

        // 更新配置中的相机校准参数
        systemConfig.configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(cameraParam);

        // 更新并保存配置
        if (!m_pConfigManager->UpdateFullConfig(systemConfig)) {
            LOG_ERROR("Failed to update config with leveling results\n");
            return false;
        }

        if (!m_pConfigManager->SaveConfigToFile(configPath.toStdString())) {
            LOG_ERROR("Failed to save config file 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;
    }
}

bool RodAndBarPositionPresenter::LoadLevelingResults(int cameraIndex, const QString& cameraName,
                                              double planeCalib[9], double& planeHeight, double invRMatrix[9])
{
    try {
        if (!m_pConfigManager) {
            LOG_ERROR("ConfigManager is null, cannot load calibration data\n");
            return false;
        }

        // 从ConfigManager获取配置结果
        ConfigResult configResult = m_pConfigManager->GetConfigResult();

        // 获取指定相机的标定参数
        VrCameraPlaneCalibParam cameraParamValue;
        if (!configResult.algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraParamValue) || !cameraParamValue.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] = cameraParamValue.planeCalib[i];
            invRMatrix[i] = cameraParamValue.invRMatrix[i];
        }
        planeHeight = cameraParamValue.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 LoadLevelingResults: %s\n", e.what());
        return false;
    }
}

// 反初始化
void RodAndBarPositionPresenter::DeinitApp()
{
    LOG_DEBUG("Deinitializing RodAndBarPositionPresenter\n");

    // 停止检测
    StopDetection();

    // 释放ConfigManager（析构函数会自动调用Shutdown）
    if (m_pConfigManager) {
        delete m_pConfigManager;
        m_pConfigManager = nullptr;
    }

    // 释放检测处理器
    if (m_pDetectPresenter) {
        delete m_pDetectPresenter;
        m_pDetectPresenter = nullptr;
    }

    LOG_DEBUG("RodAndBarPositionPresenter deinitialized\n");
}

// 触发检测
bool RodAndBarPositionPresenter::TriggerDetection(int cameraIndex)
{
    // 设置相机索引
    if (cameraIndex > 0) {
        SetDefaultCameraIndex(cameraIndex);
    }

    // 检查是否已连接相机
    if (!m_bCameraConnected) {
        LOG_WARNING("Camera not connected, cannot trigger detection\n");
        return false;
    }

    // 触发检测
    int ret = StartDetection(cameraIndex, false);
    if (ret != SUCCESS) {
        LOG_ERROR("Failed to trigger detection, error: %d\n", ret);
        return false;
    }

    return true;
}

// 加载文件并检测
int RodAndBarPositionPresenter::LoadAndDetect(const QString& fileName)
{
    LOG_INFO("Loading data from file: %s\n", fileName.toStdString().c_str());

    // 使用基类的方法加载调试数据并执行检测
    return LoadDebugDataAndDetect(fileName.toStdString());
}

// 重连相机
void RodAndBarPositionPresenter::ReconnectCamera()
{
    LOG_INFO("Attempting to reconnect cameras\n");
    TryReconnectCameras();
}

// 获取算法参数
RodAndBarPositionPresenter::AlgoParams RodAndBarPositionPresenter::GetAlgoParams() const
{
    AlgoParams params;

    if (m_pConfigManager) {
        VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
        params.rodParam = algorithmParams.rodParam;
        params.cornerParam = algorithmParams.cornerParam;
        params.filterParam = algorithmParams.filterParam;
        params.growParam = algorithmParams.growParam;
    } else {
        // 使用默认参数
        params.rodParam.rodDiameter = 52.0;
        params.rodParam.rodLen = 290.0;
    }

    return params;
}

// 设置算法参数
void RodAndBarPositionPresenter::SetAlgoParams(const AlgoParams& params)
{
    if (!m_pConfigManager) {
        LOG_WARNING("ConfigManager not initialized, cannot set algorithm params\n");
        return;
    }

    // 获取当前配置
    VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();

    // 更新参数
    algorithmParams.rodParam = params.rodParam;
    algorithmParams.cornerParam = params.cornerParam;
    algorithmParams.filterParam = params.filterParam;
    algorithmParams.growParam = params.growParam;

    // 更新到ConfigManager
    m_pConfigManager->UpdateAlgorithmParams(algorithmParams);

    LOG_INFO("Algorithm parameters updated\n");
}

// ============ ModbusTCP Server 相关实现 ============

void RodAndBarPositionPresenter::OnModbusWriteCallback(uint16_t startAddress, const uint16_t* data, uint16_t count)
{
    // 检测触发寄存器 (地址 0)：写入非零值触发检测或读取缓存结果
    if (startAddress == 0 && count >= 1 && data[0] != 0) {
        int cmd = data[0];  // 1=触发相机1, 2=触发相机2, 10=读取下一个缓存结果
        LOG_INFO("Modbus command received: cmd=%d\n", cmd);

        // 清除触发寄存器
        uint16_t clearValue = 0;
        WriteModbusRegisters(0, &clearValue, 1);

        if (cmd == 10) {
            // 读取缓存中的下一个棒材结果
            _SendNextCachedResult();
        } else {
            // 新检测触发，索引归零
            {
                std::lock_guard<std::mutex> lock(m_resultCacheMutex);
                m_resultIndex = 0;
            }
            // 设置状态为"正在检测"
            uint16_t detecting = 0;
            WriteModbusRegisters(1, &detecting, 1);
            // 触发对应相机检测
            TriggerDetection(cmd);
        }
    }
}

void RodAndBarPositionPresenter::OnModbusServerStatusChanged(bool isConnected)
{
    LOG_INFO("Modbus server status changed: %s\n", isConnected ? "connected" : "disconnected");

    if (auto pStatus = GetStatusCallback<IYRodAndBarPositionStatus>()) {
        pStatus->OnRobotConnectionChanged(isConnected);
    }
}

void RodAndBarPositionPresenter::_SendDetectionResultToModbus(const DetectionResult& result, int cameraIndex, int index)
{
    if (!IsModbusServerRunning()) {
        LOG_WARNING("Modbus server not running, skip sending result\n");
        return;
    }

    // 从配置获取字节序
    ConfigResult configResult = m_pConfigManager->GetConfigResult();
    bool bigEndian = (configResult.byteOrder == 0);  // 0=大端序, 1=小端序

    // 地址 1：状态码（0=正在检测，1=成功，2=失败，3=结果为空/无更多结果）
    uint16_t statusCode = 1;  // 默认成功
    if (!result.success) {
        statusCode = 2;  // 检测失败
    } else if (result.positions.empty() || index >= (int)result.positions.size()) {
        statusCode = 3;  // 成功但结果为空或索引越界
    }
    WriteModbusRegisters(1, &statusCode, 1);

    // 地址 2~13：指定索引棒材的 XYZ + RPY（6 个 float = 12 个寄存器）
    if (statusCode == 1) {
        const auto& pos = result.positions[index];
        float values[6] = {
            (float)pos.x, (float)pos.y, (float)pos.z,
            (float)pos.roll, (float)pos.pitch, (float)pos.yaw
        };
        uint16_t regs[12];
        memcpy(regs, values, sizeof(regs));

        // 大端序时交换每个float的高低字序（x86小端 → Modbus大端字序）
        if (bigEndian) {
            for (int i = 0; i < 6; i++) {
                std::swap(regs[i * 2], regs[i * 2 + 1]);
            }
        }

        WriteModbusRegisters(2, regs, 12);
    }

    LOG_INFO("Detection result sent via Modbus, camera: %d, index: %d, status: %d\n",
             cameraIndex, index, statusCode);
}

void RodAndBarPositionPresenter::_SendNextCachedResult()
{
    if (!IsModbusServerRunning()) {
        LOG_WARNING("Modbus server not running, skip sending cached result\n");
        return;
    }

    std::lock_guard<std::mutex> lock(m_resultCacheMutex);
    if (m_lastDetectionResult.positions.empty() ||
        m_resultIndex >= (int)m_lastDetectionResult.positions.size()) {
        // 无更多结果，写状态码3
        uint16_t statusCode = 3;
        WriteModbusRegisters(1, &statusCode, 1);
        LOG_INFO("No more cached results, index=%d, total=%zu\n",
                 m_resultIndex, m_lastDetectionResult.positions.size());
        return;
    }

    LOG_INFO("Sending cached result index=%d, total=%zu\n",
             m_resultIndex, m_lastDetectionResult.positions.size());
    _SendDetectionResultToModbus(m_lastDetectionResult,
                                  m_lastDetectionResult.cameraIndex,
                                  m_resultIndex);
    m_resultIndex++;
}