#include "GrabBagPresenter.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 <cmath>
#include <algorithm>
#include <QImage>
#include <QThread>

#include "VrTimeUtils.h"
#include "VrDateUtils.h"
#include "SG_bagPositioning_Export.h"
#include "SG_baseDataType.h"
#include "VrConvert.h"

GrabBagPresenter::GrabBagPresenter()
    : m_vrConfig(nullptr)
    , m_pStatus(nullptr)
    , m_pRobotProtocol(nullptr)
    , m_bCameraConnected(false)
    , m_bRobotConnected(false)
    , m_currentWorkStatus(WorkStatus::Error)
{

}

GrabBagPresenter::~GrabBagPresenter()
{
    // 停止算法检测线程
    m_bAlgoDetectThreadRunning = false;
    m_algoDetectCondition.notify_all();
    
    // 释放缓存的检测数据
    _ClearDetectionDataCache();
    
    // 释放机械臂协议
    if (m_pRobotProtocol) {
        m_pRobotProtocol->Deinitialize();
        delete m_pRobotProtocol;
        m_pRobotProtocol = nullptr;
    }
    
    // 释放其他资源
    for(auto it = m_vrEyeDeviceList.begin(); it != m_vrEyeDeviceList.end(); it++)
    {
        (*it)->CloseDevice();
        delete (*it);
    }
    m_vrEyeDeviceList.clear();  

}

int GrabBagPresenter::Init()
{
    // 初始化连接状态
    m_bCameraConnected = false;
    m_currentWorkStatus = WorkStatus::InitIng;


    m_pStatus->OnWorkStatusChanged(m_currentWorkStatus);
    
    // 初始化VrConfig模块
    IVrConfig::CreateInstance(&m_vrConfig);
    
    // 设置配置改变通知回调
    if (m_vrConfig) {
        m_vrConfig->SetConfigChangeNotify(this);
    }

    // 获取配置文件路径
    QString configPath = PathManager::GetConfigFilePath();
    
    // 读取IP列表
    ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
    int nRet = SUCCESS;

    // 初始化算法参数
    nRet = InitAlgorithmParams();
    if (nRet != 0) {
        m_pStatus->OnStatusUpdate("算法参数初始化失败");
        LOG_ERROR("Algorithm parameters initialization failed with error: %d\n", nRet);
    } else {
        m_pStatus->OnStatusUpdate("算法参数初始化成功");
        LOG_INFO("Algorithm parameters initialization successful\n");
    }

    // 初始化机械臂协议
    nRet = InitRobotProtocol();
    if (nRet != 0) {
        m_pStatus->OnStatusUpdate("机械臂服务初始化失败");
        m_bRobotConnected = false;
    } else {
        m_pStatus->OnStatusUpdate("机械臂服务初始化成功");
    }
    

    // 通知UI相机个数
    int cameraCount = configResult.cameraList.size();
    m_pStatus->OnCameraCountChanged(cameraCount);

    if(cameraCount > 0){
        if (cameraCount >= 1) {
            // 尝试打开相机1
            LOG_INFO("Attempting to connect to camera 1 with IP: %s\n", configResult.cameraList[0].ip.c_str());
            
            // 发送页面提示信息
            m_pStatus->OnStatusUpdate(QString("正在连接相机一，IP: %1").arg(configResult.cameraList[0].ip.c_str()).toStdString());
            
            // 初始化VrEyeDevice模块
            IVrEyeDevice* pDevice = nullptr;
            IVrEyeDevice::CreateObject(&pDevice);
            
            nRet = pDevice->InitDevice();
            ERR_CODE_RETURN(nRet);

            nRet = pDevice->OpenDevice(configResult.cameraList[0].ip.c_str(), &GrabBagPresenter::_StaticCameraNotify, this);
            // 通过回调更新相机1状态
            bool camera1Connected = (SUCCESS == nRet);
            if(camera1Connected){
                m_vrEyeDeviceList.push_back(pDevice);
                LOG_INFO("Camera 1 connection successful, IP: %s\n", configResult.cameraList[0].ip.c_str());
                m_pStatus->OnStatusUpdate(QString("相机一连接成功，IP: %1").arg(configResult.cameraList[0].ip.c_str()).toStdString());
            } else {
                LOG_ERROR("Camera 1 connection failed, IP: %s, error code: %d\n", configResult.cameraList[0].ip.c_str(), nRet);
                m_pStatus->OnStatusUpdate(QString("相机一连接失败，IP: %1，错误码: %2").arg(configResult.cameraList[0].ip.c_str()).arg(nRet).toStdString());
            }
            m_pStatus->OnCamera1StatusChanged(camera1Connected);
            m_pStatus->OnStatusUpdate(camera1Connected ? "相机一连接成功" : "相机一连接失败");
            m_bCameraConnected = camera1Connected;
        }
        else {
            LOG_WARNING("Camera count is 0, setting camera 1 as disconnected\n");
            m_pStatus->OnStatusUpdate("配置文件中未找到相机配置，设置相机一为离线状态");
            m_pStatus->OnCamera1StatusChanged(false);
            m_bCameraConnected = false;
        }
        ERR_CODE_RETURN(nRet);
        
        if (cameraCount >= 2) {
            IVrEyeDevice* pDevice = nullptr;
            IVrEyeDevice::CreateObject(&pDevice);
            nRet = pDevice->InitDevice();
            ERR_CODE_RETURN(nRet);
            // 尝试打开相机2
            nRet = pDevice->OpenDevice(configResult.cameraList[1].ip.c_str(), &GrabBagPresenter::_StaticCameraNotify, this);
            // 通过回调更新相机2状态
            bool camera2Connected = (SUCCESS == nRet);
            if(camera2Connected)
            {
                m_vrEyeDeviceList.push_back(pDevice);
            }
            m_pStatus->OnCamera2StatusChanged(camera2Connected);
            m_pStatus->OnStatusUpdate(camera2Connected ? "相机二连接成功" : "相机二连接失败");
            // 只要有一个相机连接成功就认为相机连接正常
            if (camera2Connected) {
                m_bCameraConnected = true;
            }
        }
        else {
            // 如果只有一个相机，则将相机2状态设为未连接
            m_pStatus->OnCamera2StatusChanged(false);
        }
        ERR_CODE_RETURN(nRet);
    } else {
        IVrEyeDevice* pDevice = nullptr;
        IVrEyeDevice::CreateObject(&pDevice);
        nRet = pDevice->InitDevice();
            ERR_CODE_RETURN(nRet);
        // 尝试打开相机1
        nRet = pDevice->OpenDevice(nullptr, &GrabBagPresenter::_StaticCameraNotify, this);
        // 通过回调更新相机1状态
        bool camera1Connected = (SUCCESS == nRet);
        if(camera1Connected)
        {
            m_vrEyeDeviceList.push_back(pDevice);
        }
        m_pStatus->OnCamera1StatusChanged(camera1Connected);
        m_pStatus->OnStatusUpdate(camera1Connected ? "相机一连接成功" : "相机一连接失败");
        m_bCameraConnected = camera1Connected;
    }

    m_bAlgoDetectThreadRunning = true;
    std::thread algoDetectThread(&GrabBagPresenter::_AlgoDetectThread, this);
    algoDetectThread.detach();

    m_pStatus->OnStatusUpdate("设备初始化完成");

    CheckAndUpdateWorkStatus();
    
    return SUCCESS;
}

// 初始化机械臂协议
int GrabBagPresenter::InitRobotProtocol()
{
    LOG_DEBUG("Start initializing robot protocol\n");
    m_pStatus->OnStatusUpdate("机械臂服务初始化...");
    
    // 创建RobotProtocol实例
    if(nullptr == m_pRobotProtocol){
        m_pRobotProtocol = new RobotProtocol();
    }

    // 初始化协议服务（使用端口502）
    int nRet = m_pRobotProtocol->Initialize(5020);

    // 设置连接状态回调
    m_pRobotProtocol->SetConnectionCallback([this](bool connected) {
        this->OnRobotConnectionChanged(connected);
    });

    // 设置工作信号回调
    m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork, int cameraIndex) {
        return this->OnRobotWorkSignal(startWork, cameraIndex);
    });
    

    LOG_INFO("Robot protocol initialization completed successfully\n");
    return nRet;
    
}

// 初始化算法参数
int GrabBagPresenter::InitAlgorithmParams()
{
    LOG_DEBUG("Start initializing algorithm parameters\n");
    QString exePath = QCoreApplication::applicationFilePath();

    // 初始化手眼标定矩阵为单位矩阵（4x4变换矩阵）
    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   // 第四行
    };
    for (int i = 0; i < 16; i++) {
        m_clibMatrix[i] = initClibMatrix[i];
    }
    
    // 获取手眼标定文件路径并确保文件存在
    QString clibPath = PathManager::GetCalibrationFilePath();
    
    LOG_INFO("Loading hand-eye calibration matrix from: %s\n", clibPath.toStdString().c_str());
    if(QFile::exists(clibPath))
    {
        CVrConvert::LoadClibMatrix(clibPath.toStdString().c_str(), "clib", m_clibMatrix);
    }
    
    // 获取配置文件路径
    QString configPath = PathManager::GetConfigFilePath();
    
    LOG_INFO("Loading configuration from: %s\n", configPath.toStdString().c_str());
    
    // 检查配置文件是否存在
    QFileInfo configFileInfo(configPath);
    if (!configFileInfo.exists()) {
        LOG_ERROR("Configuration file does not exist: %s\n", configPath.toStdString().c_str());
        return ERR_CODE(FILE_ERR_NOEXIST);
    }
    
    // 读取配置文件
    ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
    const VrAlgorithmParams& xmlParams = configResult.algorithmParams;
    
    // 保存调试参数
    m_debugParam = configResult.debugParam;
    
    LOG_INFO("Loaded XML params - Bag: L=%.1f, W=%.1f, H=%.1f\n", 
             xmlParams.bagParam.bagL, xmlParams.bagParam.bagW, xmlParams.bagParam.bagH);
    LOG_INFO("Loaded XML params - Pile: L=%.1f, W=%.1f, H=%.1f\n", 
             xmlParams.pileParam.pileL, xmlParams.pileParam.pileW, xmlParams.pileParam.pileH);
    
    // 初始化算法参数结构
    memset(&m_algoParam, 0, sizeof(SG_bagPositionParam));
    
    // 设置编织袋参数
    m_algoParam.bagParam.bagL = xmlParams.bagParam.bagL;
    m_algoParam.bagParam.bagW = xmlParams.bagParam.bagW;
    m_algoParam.bagParam.bagH = xmlParams.bagParam.bagH;
    
    // 设置滤波参数
    m_algoParam.filterParam.continuityTh = xmlParams.filterParam.continuityTh;
    m_algoParam.filterParam.outlierTh = xmlParams.filterParam.outlierTh;
    
    // 设置角点特征参数
    m_algoParam.cornerParam.cornerTh        = xmlParams.cornerParam.cornerTh;
    m_algoParam.cornerParam.scale           = xmlParams.cornerParam.scale;
    m_algoParam.cornerParam.minEndingGap    = xmlParams.cornerParam.minEndingGap;
    m_algoParam.cornerParam.jumpCornerTh_1  = xmlParams.cornerParam.jumpCornerTh_1;
    m_algoParam.cornerParam.jumpCornerTh_2  = xmlParams.cornerParam.jumpCornerTh_2;
    
    // 设置增长参数
    m_algoParam.growParam.maxLineSkipNum = xmlParams.growParam.maxLineSkipNum;
    m_algoParam.growParam.yDeviation_max = xmlParams.growParam.yDeviation_max;
    m_algoParam.growParam.maxSkipDistance = xmlParams.growParam.maxSkipDistance;
    m_algoParam.growParam.zDeviation_max = xmlParams.growParam.zDeviation_max;
    m_algoParam.growParam.minLTypeTreeLen = xmlParams.growParam.minLTypeTreeLen;
    m_algoParam.growParam.minVTypeTreeLen = xmlParams.growParam.minVTypeTreeLen;
    
    // 设置平面校准参数（使用从XML配置文件读取的数据）
    for (int i = 0; i < 9; i++) {
        m_planeCalibParam.planeCalib[i] = xmlParams.planeCalibParam.planeCalib[i];
        m_planeCalibParam.invRMatrix[i] = xmlParams.planeCalibParam.invRMatrix[i];
    }
    m_planeCalibParam.planeHeight = xmlParams.planeCalibParam.planeHeight;
    
    LOG_INFO("Algorithm parameters initialized successfully:\n");
    LOG_INFO("  Bag: L=%.1f, W=%.1f, H=%.1f\n", m_algoParam.bagParam.bagL, m_algoParam.bagParam.bagW, m_algoParam.bagParam.bagH);
    LOG_INFO("  Filter: continuityTh=%.1f, outlierTh=%d\n", m_algoParam.filterParam.continuityTh, m_algoParam.filterParam.outlierTh);
    LOG_INFO("  Plane calibration: height=%.1f\n", m_planeCalibParam.planeHeight);
    LOG_INFO("  Plane calibration matrix:\n");
    LOG_INFO("    [%.3f, %.3f, %.3f]\n", m_planeCalibParam.planeCalib[0], m_planeCalibParam.planeCalib[1], m_planeCalibParam.planeCalib[2]);
    LOG_INFO("    [%.3f, %.3f, %.3f]\n", m_planeCalibParam.planeCalib[3], m_planeCalibParam.planeCalib[4], m_planeCalibParam.planeCalib[5]);
    LOG_INFO("    [%.3f, %.3f, %.3f]\n", m_planeCalibParam.planeCalib[6], m_planeCalibParam.planeCalib[7], m_planeCalibParam.planeCalib[8]);
    LOG_INFO("  Inverse rotation matrix:\n");
    LOG_INFO("    [%.3f, %.3f, %.3f]\n", m_planeCalibParam.invRMatrix[0], m_planeCalibParam.invRMatrix[1], m_planeCalibParam.invRMatrix[2]);
    LOG_INFO("    [%.3f, %.3f, %.3f]\n", m_planeCalibParam.invRMatrix[3], m_planeCalibParam.invRMatrix[4], m_planeCalibParam.invRMatrix[5]);
    LOG_INFO("    [%.3f, %.3f, %.3f]\n", m_planeCalibParam.invRMatrix[6], m_planeCalibParam.invRMatrix[7], m_planeCalibParam.invRMatrix[8]);
    
    return SUCCESS;
}

// 机械臂协议回调函数实现
void GrabBagPresenter::OnRobotConnectionChanged(bool connected)
{
    LOG_INFO("Robot connection status changed: %s\n", (connected ? "connected" : "disconnected"));
    
    // 更新机械臂连接状态
    m_bRobotConnected = connected;
    
    if (m_pStatus) {
        m_pStatus->OnRobotConnectionChanged(connected);
        
        // 发送详细的页面状态信息
        if (connected) {
            m_pStatus->OnStatusUpdate("机械臂连接成功，通信正常");
        } else {
            m_pStatus->OnStatusUpdate("机械臂连接断开，请检查网络连接");
        }
    }
    
    // 检查并更新工作状态
    CheckAndUpdateWorkStatus();
}

bool GrabBagPresenter::OnRobotWorkSignal(bool startWork, int cameraIndex)
{
    LOG_INFO("Received robot work signal: %s for camera index: %d\n", (startWork ? "start work" : "stop work"), cameraIndex);
    
    int nRet = SUCCESS;
    
    if (startWork) {
        QString message = QString("收到开始工作信号，启动相机 %1 检测").arg(cameraIndex);
        if (nullptr != m_pStatus) {
            m_pStatus->OnStatusUpdate(message.toStdString());
        }
        nRet = StartDetection(cameraIndex);
    } else {
        QString message = QString("收到停止工作信号，停止相机 %1 检测").arg(cameraIndex);
        if (nullptr != m_pStatus) {
            m_pStatus->OnStatusUpdate(message.toStdString());
        }
        nRet = StopDetection();
    }

    return nRet == SUCCESS;
}

void GrabBagPresenter::SetStatusCallback(IYGrabBagStatus* status)
{
    m_pStatus = status;
}

// 模拟检测函数，用于演示
int GrabBagPresenter::StartDetection(int cameraIndex, bool isAuto)
{    
    LOG_INFO("--------------------------------\n");
    LOG_INFO("Start detection with camera index: %d\n", cameraIndex);
    
    // 检查设备状态是否准备就绪
    if (isAuto && m_currentWorkStatus != WorkStatus::Ready) {
        LOG_INFO("Device not ready, cannot start detection\n");
        if (m_pStatus) {
            m_pStatus->OnStatusUpdate("设备未准备就绪，无法开始检测");
        }
        return ERR_CODE(DEV_BUSY);
    }

    // 保存当前使用的相机ID（从1开始编号）
    m_currentCameraIndex = cameraIndex;

    // 通知UI工作状态变更为"正在工作"
    if (m_pStatus) {
        m_currentWorkStatus = WorkStatus::Working;
        m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
    }
    
    // 设置机械臂工作状态为忙碌
    if (m_pRobotProtocol) {
        m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_BUSY);
    }

    if(m_vrEyeDeviceList.empty()){
        LOG_ERROR("No camera device found\n");
        if (m_pStatus) {
            m_pStatus->OnStatusUpdate("未找到相机设备");
        }
        return ERR_CODE(DEV_NOT_FIND);
    }

    // 清空检测数据缓存（释放之前的内存）
    _ClearDetectionDataCache();

    int nRet = SUCCESS;
    
    // 根据参数决定启动哪些相机
    
    // 启动指定相机（cameraIndex为相机ID，从1开始编号）
    int arrayIndex = cameraIndex - 1;  // 转换为数组索引（从0开始）
    if (arrayIndex >= 0 && arrayIndex < static_cast<int>(m_vrEyeDeviceList.size())) {
        nRet = m_vrEyeDeviceList[arrayIndex]->StartDetect(&GrabBagPresenter::_StaticDetectionCallback, this);
        if (nRet == SUCCESS) {
            LOG_INFO("Camera ID %d start detection success\n", cameraIndex);
            m_pStatus->OnStatusUpdate(QString("启动相机 %1 检测成功").arg(cameraIndex).toStdString());
        } else {
            LOG_ERROR("Camera ID %d start detection failed with error: %d\n", cameraIndex, nRet);
            m_pStatus->OnStatusUpdate(QString("启动相机 %1 检测失败").arg(cameraIndex).toStdString());
        }
    } else {
        LOG_ERROR("Invalid camera ID: %d, valid range is 1-%zu\n", cameraIndex, m_vrEyeDeviceList.size());
        m_pStatus->OnStatusUpdate(QString("无效的相机ID: %1，有效范围: 1-%2").arg(cameraIndex).arg(m_vrEyeDeviceList.size()).toStdString());
        nRet = ERR_CODE(DEV_NOT_FIND);
    }
    
    return nRet;
}

int GrabBagPresenter::StopDetection()
{
    LOG_INFO("Stop detection\n");
    
    // 停止所有相机的检测
    for (size_t i = 0; i < m_vrEyeDeviceList.size(); ++i) {
        IVrEyeDevice* pDevice = m_vrEyeDeviceList[i];
        if (pDevice) {
            int ret = pDevice->StopDetect();
            if (ret == 0) {
                LOG_INFO("Camera %zu stop detection successfully\n", i + 1);
            } else {
                LOG_WARNING("Camera %zu stop detection failed, error code: %d\n", i + 1, ret);
            }
        }
    }
    
    // 通知UI工作状态变更为"就绪"（如果设备连接正常）
    if (m_pStatus) {
        // 检查设备连接状态，决定停止后的状态
        if (m_bCameraConnected && m_bRobotConnected) {
            m_currentWorkStatus = WorkStatus::Ready;
            m_pStatus->OnWorkStatusChanged(WorkStatus::Ready);
        } else {
            m_currentWorkStatus = WorkStatus::Error;
            m_pStatus->OnWorkStatusChanged(WorkStatus::Error);
        }
        m_pStatus->OnStatusUpdate("检测已停止");
    }
    
    // 设置机械臂工作状态为空闲
    if (m_pRobotProtocol) {
        m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_IDLE);
    }

    return 0;
}

// 加载调试数据进行检测
int GrabBagPresenter::LoadDebugDataAndDetect(const std::string& filePath)
{
    LOG_INFO("Loading debug data from file: %s\n", filePath.c_str());
    
    m_currentWorkStatus = WorkStatus::Working;
    m_currentCameraIndex = 1;

    if (m_pStatus) {
        m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
        m_pStatus->OnStatusUpdate(QString("正加载: %1").arg(QString::fromStdString(filePath)).toStdString());
    }
    
    // 设置机械臂工作状态为忙碌
    if (m_pRobotProtocol) {
        m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_BUSY);
    }

    
    int lineNum = 0;
    float scanSpeed = 0.0f;
    int maxTimeStamp = 0;
    int clockPerSecond = 0;
    

    int result = SUCCESS;
    // 清空现有的检测数据缓存
    _ClearDetectionDataCache();
    {
        std::lock_guard<std::mutex> lock(m_detectionDataMutex);
        result = m_dataLoader.LoadLaserScanData(filePath, m_detectionDataCache, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
    }
    if (result != SUCCESS) {
        LOG_ERROR("Failed to load debug data: %s\n", m_dataLoader.GetLastError().c_str());
        if (m_pStatus) {
            m_pStatus->OnStatusUpdate("调试数据加载失败: " + m_dataLoader.GetLastError());
        }
        return result;
    }
    
    LOG_INFO("Successfully loaded %d lines of debug data\n", lineNum);
    if (m_pStatus) {
        m_pStatus->OnStatusUpdate(QString("成功加载 %1 行调试数据").arg(lineNum).toStdString());
    }
        
    // 等待检测完成（这里可以考虑异步处理）
    // 为了简化，这里直接调用检测任务
    result = _DetectTask();
    
    // 释放加载的数据
    m_dataLoader.FreeLaserScanData(m_detectionDataCache);
    
    return result;
}

IVrEyeDevice *GrabBagPresenter::GetEyeDevice(int index)
{
    if(m_vrEyeDeviceList.size() <= index) return nullptr;
    return m_vrEyeDeviceList[index];
}

// 获取所有相机设备列表
std::vector<IVrEyeDevice*> GrabBagPresenter::GetCameraList()
{
    return m_vrEyeDeviceList;
}

// 获取相机数量
int GrabBagPresenter::GetCameraCount()
{
    return static_cast<int>(m_vrEyeDeviceList.size());
}

// 获取相机列表和名称
void GrabBagPresenter::GetCameraListWithNames(std::vector<IVrEyeDevice*>& cameraList, std::vector<QString>& cameraNames)
{
    cameraList.clear();
    cameraNames.clear();
    
    for (size_t i = 0; i < m_vrEyeDeviceList.size(); i++) {
        if (m_vrEyeDeviceList[i] != nullptr) {
            cameraList.push_back(m_vrEyeDeviceList[i]);
            cameraNames.push_back(QString("相机 %1").arg(i + 1));
        }
    }
}

// 检查指定索引的相机是否连接
bool GrabBagPresenter::IsCameraConnected(int index)
{
    if (index < 0 || index >= static_cast<int>(m_vrEyeDeviceList.size())) {
        return false;
    }
    
    IVrEyeDevice* pDevice = m_vrEyeDeviceList[index];
    return (pDevice != nullptr);
}


// 静态回调函数实现
void GrabBagPresenter::_StaticCameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
    // 从pInfoParam获取this指针，转换回GrabBagPresenter*类型
    GrabBagPresenter* pThis = reinterpret_cast<GrabBagPresenter*>(pInfoParam);
    if (pThis)
    {
        // 调用实例的非静态成员函数
        pThis->_CameraNotify(eStatus, pExtData, nDataLength, pInfoParam);
    }
}

void GrabBagPresenter::_CameraNotify(EVzDeviceWorkStatus eStatus, void *pExtData, unsigned int nDataLength, void *pInfoParam)
{
    LOG_DEBUG("[Camera Notify] received: status=%d\n", (int)eStatus);
    
    switch (eStatus) {
        case EVzDeviceWorkStatus::keDeviceWorkStatus_Offline:
        {
            LOG_WARNING("[Camera Notify] Camera device offline/disconnected\n");
            
            // 更新相机连接状态
            m_bCameraConnected = false;
            
            // 通知UI相机状态变更
            if (m_pStatus) {
                // 这里需要判断是哪个相机离线，暂时更新相机1状态
                // 实际应用中可能需要通过pInfoParam或其他方式区分具体哪个相机
                m_pStatus->OnCamera1StatusChanged(false);
                m_pStatus->OnStatusUpdate("相机设备离线");
            }
            
            // 检查并更新工作状态
            CheckAndUpdateWorkStatus();
            break;
        }
        
        case EVzDeviceWorkStatus::keDeviceWorkStatus_Eye_Reconnect:
        {
            LOG_INFO("[Camera Notify] Camera device online/connected\n");
            
            // 更新相机连接状态
            m_bCameraConnected = true;
            
            // 通知UI相机状态变更
            if (m_pStatus) {
                m_pStatus->OnCamera1StatusChanged(true);
                m_pStatus->OnStatusUpdate("相机设备已连接");
            }
            
            // 检查并更新工作状态
            CheckAndUpdateWorkStatus();
            break;
        }
        
        case EVzDeviceWorkStatus::keDeviceWorkStatus_Device_Swing_Finish:
        {
            LOG_INFO("[Camera Notify] Received scan finish signal from camera\n");
            
            // 发送页面提示信息
            if (m_pStatus) {
                m_pStatus->OnStatusUpdate("相机扫描完成，开始数据处理...");
            }
            
            // 通知检测线程开始处理
            m_algoDetectCondition.notify_one();
            break;
        }
        
        default:
            break;
    }
}

// 检测数据回调函数静态版本
void GrabBagPresenter::_StaticDetectionCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pUserData)
{
    GrabBagPresenter* pThis = reinterpret_cast<GrabBagPresenter*>(pUserData);
    if (pThis && pLaserLinePoint) {
        pThis->_DetectionCallback(eDataType, pLaserLinePoint);
    }
}

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

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

    if (!pLaserLinePoint->p3DPoint) {
        LOG_WARNING("[Detection 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_detectionDataMutex);
    m_detectionDataCache.push_back(laser3DLine);
}

void GrabBagPresenter::CheckAndUpdateWorkStatus()
{
    if (m_bCameraConnected && m_bRobotConnected) {
        m_currentWorkStatus = WorkStatus::Ready;
        m_pStatus->OnWorkStatusChanged(WorkStatus::Ready);
    } else {
        m_currentWorkStatus = WorkStatus::Error;
        m_pStatus->OnWorkStatusChanged(WorkStatus::Error);
    }
}

void GrabBagPresenter::_AlgoDetectThread()
{
    while(m_bAlgoDetectThreadRunning)
    {
        std::unique_lock<std::mutex> lock(m_algoDetectMutex);
        m_algoDetectCondition.wait(lock, [this]() {
            return m_currentWorkStatus == WorkStatus::Working;
        });

        // 检查设备状态是否准备就绪
        int nRet = _DetectTask();

        if(nRet != SUCCESS){
            m_currentWorkStatus = WorkStatus::Error;
            if(m_pStatus){
                m_pStatus->OnWorkStatusChanged(m_currentWorkStatus);
            }
        }
    }
}

int GrabBagPresenter::_DetectTask()
{   
    LOG_INFO("[Algo Thread] Start real detection task using algorithm\n");
    
    // 1. 获取缓存的点云数据
    if (m_detectionDataCache.empty()) {
        LOG_WARNING("No cached detection data available\n");
        if (m_pStatus) {
            m_pStatus->OnStatusUpdate("无缓存的检测数据");
        }
        return ERR_CODE(DEV_DATA_INVALID);
    }
    
    // 2. 准备算法输入数据
    if(m_pStatus){
        m_pStatus->OnStatusUpdate("扫描线数：" + std::to_string(m_detectionDataCache.size()) + "，正在进行算法检测...");
    }

    if(m_debugParam.savePointCloud){
        LOG_INFO("[Algo Thread] Debug mode is enabled\n");
        
        // 获取当前时间戳，格式为YYYYMMDDHHMMSS
        std::string timeStamp = CVrDateUtils::GetNowTime();
        
        std::string fileName = m_debugParam.debugOutputPath + "/pointCloud_" + timeStamp + ".txt";
        m_dataLoader.SaveLaserScanData(fileName, m_detectionDataCache, m_detectionDataCache.size(), 0.0, 0, 0);
    }
    
    // 3. 使用成员变量算法参数（已在初始化时从XML读取）
    LOG_INFO("[Algo Thread] Using algorithm parameters from XML configuration\n");
    LOG_INFO("  Bag: L=%.1f, W=%.1f, H=%.1f\n",m_algoParam.bagParam.bagL, m_algoParam.bagParam.bagW, m_algoParam.bagParam.bagH);
    LOG_INFO("  Filter: continuityTh=%.1f, outlierTh=%d\n", m_algoParam.filterParam.continuityTh, m_algoParam.filterParam.outlierTh);
    
    CVrTimeUtils oTimeUtils;
    // 4. 数据预处理：调平和去除地面（使用成员变量平面校准参数）
    for (size_t i = 0; i < m_detectionDataCache.size(); i++) {
        sg_lineDataR(&m_detectionDataCache[i], m_planeCalibParam.planeCalib, m_planeCalibParam.planeHeight);
    }

    // 5. 调用算法检测接口（使用成员变量参数）
    std::vector<SSG_peakRgnInfo> objOps;
    sg_getBagPosition(static_cast<SVzNL3DLaserLine*>(m_detectionDataCache.data()), m_detectionDataCache.size(), m_algoParam, m_planeCalibParam, objOps);
    
    LOG_INFO("[Algo Thread] sg_getBagPosition detected %zu objects time : %.2f ms\n", objOps.size(), oTimeUtils.GetElapsedTimeInMilliSec());
    
    // 6. 转换检测结果为UI显示格式
    DetectionResult detectionResult;
    // 从点云数据生成投影图像
    detectionResult.image = _GeneratePointCloudImage(static_cast<SVzNL3DLaserLine*>(m_detectionDataCache.data()),
                                                     m_detectionDataCache.size(), objOps);
    
    // 转换检测结果为UI显示格式（使用机械臂坐标系数据）
    for (size_t i = 0; i < objOps.size(); i++) {
        const SSG_peakRgnInfo& obj = objOps[i];
        
        // 创建层数据（简化处理，将所有目标放在同一层）
        if (detectionResult.positionLayout.empty()) {
            GrabBagPositionLayout layer;
            layer.layerIndex = 1;
            detectionResult.positionLayout.push_back(layer);
        }
        
        // 进行坐标转换：从算法坐标系转换到机械臂坐标系
        SVzNL3DPoint targetObj;
        targetObj.x = obj.centerPos.x;
        targetObj.y = obj.centerPos.y;
        targetObj.z = obj.centerPos.z;
        
        SVzNL3DPoint robotObj;
        CVrConvert::EyeToRobot(targetObj, robotObj, m_clibMatrix);
        
        // 创建位置数据（使用转换后的机械臂坐标）
        GrabBagPosition pos;
        pos.x       = robotObj.x;           // 机械臂坐标X
        pos.y       = robotObj.y;           // 机械臂坐标Y
        pos.z       = robotObj.z;           // 机械臂坐标Z
        pos.roll    = obj.centerPos.x_roll; // 保持原有姿态角
        pos.pitch   = obj.centerPos.y_pitch; // 保持原有姿态角
        pos.yaw     = obj.centerPos.z_yaw;  // 保持原有偏航角
        
        detectionResult.positionLayout[0].position.push_back(pos);
        
        LOG_INFO("[Algo Thread] Object %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", 
                 i, obj.centerPos.x, obj.centerPos.y, obj.centerPos.z);
        LOG_INFO("[Algo Thread] Object %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n", 
                 i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
    }
    
    // 8. 返回检测结果
    // 调用检测结果回调函数
    m_pStatus->OnDetectionResult(detectionResult);
    
    // 更新状态
    QString statusMsg = QString("检测完成，发现%1个目标").arg(objOps.size());
    m_pStatus->OnStatusUpdate(statusMsg.toStdString());
    
    // 更新机械臂协议状态（发送转换后的目标位置数据）
    _SendDetectionResultToRobot(detectionResult, m_currentCameraIndex);
    
    // 9. 检测完成后，将工作状态更新为"完成"
    if (m_pStatus) {
        m_currentWorkStatus = WorkStatus::Completed;
        m_pStatus->OnWorkStatusChanged(WorkStatus::Completed);
    }
    
    // 设置机械臂工作状态为相应相机工作完成（相机ID从1开始）
    if (m_pRobotProtocol) {
        uint16_t workStatus = (m_currentCameraIndex == 1) ? 
            RobotProtocol::WORK_STATUS_CAMERA1_DONE : 
            RobotProtocol::WORK_STATUS_CAMERA2_DONE;
        m_pRobotProtocol->SetWorkStatus(workStatus);
    }
    
    // 恢复到就绪状态
    m_currentWorkStatus = WorkStatus::Ready;
    
    return SUCCESS;
}

// 释放缓存的检测数据
void GrabBagPresenter::_ClearDetectionDataCache()
{
    std::lock_guard<std::mutex> lock(m_detectionDataMutex);
    
    LOG_DEBUG("Clearing detection data cache, current size: %zu\n", m_detectionDataCache.size());
    
    // 释放缓存的内存
    for (auto& cachedLine : m_detectionDataCache) {
        if (cachedLine.p3DPosition) {
            delete[] cachedLine.p3DPosition;
            cachedLine.p3DPosition = nullptr;
        }
    }
    
    // 清空缓存容器
    m_detectionDataCache.clear();
    
    LOG_DEBUG("Detection data cache cleared successfully\n");
}

// 发送检测结果给机械臂
void GrabBagPresenter::_SendDetectionResultToRobot(const DetectionResult& detectionResult, int cameraIndex)
{
    if (!m_pRobotProtocol) {
        LOG_WARNING("[Robot Task] Robot protocol not initialized, cannot send detection result\n");
        return;
    }
    
    // 准备多目标数据结构
    RobotProtocol::MultiTargetData multiTargetData;
    
    // 检查是否有检测结果
    if (detectionResult.positionLayout.empty() || detectionResult.positionLayout[0].position.empty()) {
        LOG_INFO("[Robot Task] No objects detected, sending empty result to robot\n");
        // 发送空的多目标数据
        multiTargetData.count = 0;
        multiTargetData.targets.clear();

        if (m_pStatus) {
            m_pStatus->OnStatusUpdate("没有检测到目标，发送空的多目标数据给机械臂");
        }
        return;
    }
    
    // 获取检测到的目标位置（已经是机械臂坐标系）
    const auto& positions = detectionResult.positionLayout[0].position;
    multiTargetData.count = static_cast<uint16_t>(positions.size());

    // 直接使用已经转换好的机械臂坐标
    for (size_t i = 0; i < positions.size(); i++) {
        const GrabBagPosition& pos = positions[i];
        
        // 直接使用已转换的坐标数据
        RobotProtocol::TargetPosition robotTarget;
        robotTarget.x = pos.x;      // 已转换的X轴坐标
        robotTarget.y = pos.y;      // 已转换的Y轴坐标
        robotTarget.z = pos.z;      // 已转换的Z轴坐标
        robotTarget.rz = pos.yaw;   // Yaw角

        // 添加到多目标数据
        multiTargetData.targets.push_back(robotTarget);
    }
    
    // 发送多目标数据给机械臂（使用相机ID，从1开始编号）
    int result = m_pRobotProtocol->SetMultiTargetData(multiTargetData, cameraIndex);
    LOG_INFO("[Robot Task] SetMultiTargetData result: %d for camera ID: %d\n", result, cameraIndex);
    if (m_pStatus) {
        if (result != SUCCESS) {
            m_pStatus->OnStatusUpdate(QString("发送多目标坐标给机械臂失败，相机ID: %1").arg(cameraIndex).toStdString());
        } else {
            m_pStatus->OnStatusUpdate(QString("发送多目标坐标给机械臂成功，相机ID: %1").arg(cameraIndex).toStdString());
        }
    }
}

// 点云转图像 - 简化版本
QImage GrabBagPresenter::_GeneratePointCloudImage(SVzNL3DLaserLine* scanData, int lineNum, const std::vector<SSG_peakRgnInfo>& objOps)
{
    if (!scanData || lineNum <= 0) {
        return QImage();  // 返回空图像
    }
    
    // 统计X和Y的范围
    SVzNLRangeD x_range = {0, -1};
    SVzNLRangeD y_range = {0, -1};
    
    for (int line = 0; line < lineNum; line++) {
        for (int i = 0; i < scanData[line].nPositionCnt; i++) {
            SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
            if (pt3D->pt3D.z < 1e-4) continue;

            if (x_range.max < x_range.min) {
                x_range.min = x_range.max = pt3D->pt3D.x;
            } else {
                if (x_range.min > pt3D->pt3D.x) x_range.min = pt3D->pt3D.x;
                if (x_range.max < pt3D->pt3D.x) x_range.max = pt3D->pt3D.x;
            }
            if (y_range.max < y_range.min) {
                y_range.min = y_range.max = pt3D->pt3D.y;
            } else {
                if (y_range.min > pt3D->pt3D.y) y_range.min = pt3D->pt3D.y;
                if (y_range.max < pt3D->pt3D.y) y_range.max = pt3D->pt3D.y;
            }
        }
    }

    // 创建图像
    int imgCols = 800;
    int imgRows = 600;
    double x_cols = 768.0;
    double y_rows = 568.0;
    int x_skip = 16;
    int y_skip = 16;
    
    // 计算投影比例
    double x_scale = (x_range.max - x_range.min) / x_cols;
    double y_scale = (y_range.max - y_range.min) / y_rows;
    
    QImage image(imgCols, imgRows, QImage::Format_RGB888);
    image.fill(Qt::black);
    QPainter painter(&image);
    
    // 定义颜色
    QColor objColors[8] = {
        QColor(245,222,179), QColor(210,105,30), QColor(240,230,140), QColor(135,206,235),
        QColor(250,235,215), QColor(189,252,201), QColor(221,160,221), QColor(188,143,143)
    };
    
    // 绘制点云
    for (int line = 0; line < lineNum; line++) {
        for (int i = 0; i < scanData[line].nPositionCnt; i++) {
            SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
            if (pt3D->pt3D.z < 1e-4) continue;

            // 解析点索引信息
            int vType = pt3D->nPointIdx & 0xff;
            int hType = vType >> 4;
            int objId = (pt3D->nPointIdx >> 16) & 0xff;
            vType = vType & 0x0f;
            
            // 根据线特征类型确定颜色和大小
            QColor pointColor;
            int pointSize = 1;
            
            // 优先根据垂直方向特征设置颜色
            if (LINE_FEATURE_L_JUMP_H2L == vType) {
                pointColor = QColor(255, 97, 0);   // 橙色
                pointSize = 2;
            }
            else if (LINE_FEATURE_L_JUMP_L2H == vType) {
                pointColor = QColor(255, 255, 0);  // 黄色
                pointSize = 2;
            }
            else if (LINE_FEATURE_V_SLOPE == vType) {
                pointColor = QColor(255, 0, 255);  // 紫色
                pointSize = 2;
            }
            else if (LINE_FEATURE_L_SLOPE_H2L == vType) {
                pointColor = QColor(160, 82, 45);  // 褐色
                pointSize = 2;
            }
            else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) {
                pointColor = QColor(255, 0, 0);    // 红色
                pointSize = 2;
            }
            else if (LINE_FEATURE_L_SLOPE_L2H == vType) {
                pointColor = QColor(233, 150, 122); // 浅褐色
                pointSize = 2;
            }
            // 检查水平方向特征
            else if (LINE_FEATURE_L_JUMP_H2L == hType) {
                pointColor = QColor(0, 0, 255);    // 蓝色
                pointSize = 2;
            }
            else if (LINE_FEATURE_L_JUMP_L2H == hType) {
                pointColor = QColor(0, 255, 255);  // 青色
                pointSize = 2;
            }
            else if (LINE_FEATURE_V_SLOPE == hType) {
                pointColor = QColor(0, 255, 0);    // 绿色
                pointSize = 2;
            }
            else if (LINE_FEATURE_L_SLOPE_H2L == hType) {
                pointColor = QColor(85, 107, 47);  // 橄榄绿
                pointSize = 2;
            }
            else if (LINE_FEATURE_L_SLOPE_L2H == hType) {
                pointColor = QColor(0, 255, 154);  // 浅绿色
                pointSize = 2;
            }
            else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) {
                pointColor = QColor(255, 0, 0);    // 红色
                pointSize = 3;
            }
            // 检查是否为目标对象
            else if (objId > 0) {
                pointColor = objColors[objId % 8];
                pointSize = 1;
            }
            // 默认颜色
            else {
                pointColor = QColor(60, 60, 60);   // 深灰色
                pointSize = 1;
            }

            int px = (int)((pt3D->pt3D.x - x_range.min) / x_scale + x_skip);
            int py = (int)((pt3D->pt3D.y - y_range.min) / y_scale + y_skip);
            
            if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
                painter.setPen(QPen(pointColor, 1));
                painter.drawPoint(px, py);
            }
        }
    }
    
    // 绘制检测目标和方向线
    for (size_t i = 0; i < objOps.size(); i++) {
        QColor objColor = (i == 0) ? QColor(255, 0, 0) : QColor(255, 255, 0);
        int size = (i == 0) ? 12 : 8;
        
        int px = (int)((objOps[i].centerPos.x - x_range.min) / x_scale + x_skip);
        int py = (int)((objOps[i].centerPos.y - y_range.min) / y_scale + y_skip);
        
        if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
            // 绘制抓取点圆圈
            painter.setPen(QPen(objColor, 2));
            painter.setBrush(QBrush(objColor));
            painter.drawEllipse(px - size/2, py - size/2, size, size);
            
            // 绘制方向线
            const double deg2rad = PI / 180.0;
            
            // 使用检测目标实际2D尺寸的较大值的一半作为方向线长度

            // 使用较大值的一半作为方向线长度，最小值设为20以确保可见性
            double maxSize = std::max(objOps[i].objSize.dHeight, objOps[i].objSize.dWidth);
            double R = std::max(20.0, maxSize / 6.0);

            const double yaw = objOps[i].centerPos.z_yaw * deg2rad;
            double cy = cos(yaw); 
            double sy = sin(yaw);
            double x1 = objOps[i].centerPos.x + R * cy; double y1 = objOps[i].centerPos.y - R * sy;
            double x2 = objOps[i].centerPos.x - R * cy; double y2 = objOps[i].centerPos.y + R * sy;
            int px1 = (int)((x1 - x_range.min) / x_scale + x_skip);
            int py1 = (int)((y1 - y_range.min) / y_scale + y_skip);
            int px2 = (int)((x2 - x_range.min) / x_scale + x_skip);
            int py2 = (int)((y2 - y_range.min) / y_scale + y_skip);
            
            // 绘制方向线
            painter.setPen(QPen(objColor, 3));
            painter.drawLine(px1, py1, px2, py2);
            
            // 绘制目标编号
            painter.setPen(QPen(Qt::white, 1));
            painter.setFont(QFont("Arial", 10, QFont::Bold));
            painter.drawText(px + 15, py - 10, QString("%1").arg(i + 1));

        }
    }
    
    return image;
}

// 实现配置改变通知接口
void GrabBagPresenter::OnConfigChanged(const ConfigResult& configResult)
{
    LOG_INFO("Configuration changed notification received, reloading algorithm parameters\n");
    
    // 更新调试参数
    m_debugParam = configResult.debugParam;
    
    // 重新初始化算法参数
    int result = InitAlgorithmParams();
    if (result == SUCCESS) {
        LOG_INFO("Algorithm parameters reloaded successfully after config change\n");
        if (m_pStatus) {
            m_pStatus->OnStatusUpdate("配置已更新，算法参数重新加载成功");
        }
    } else {
        LOG_ERROR("Failed to reload algorithm parameters after config change, error: %d\n", result);
        if (m_pStatus) {
            m_pStatus->OnStatusUpdate("配置更新后算法参数重新加载失败");
        }
    }
}