GrabBag/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp

#include "GrabBagPresenter.h"
#include "VrError.h"
#include "VrSimpleLog.h"
#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QDir>

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

}

GrabBagPresenter::~GrabBagPresenter()
{
    // 释放机械臂协议
    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_bRobotConnected = false;
    m_currentWorkStatus = WorkStatus::Error;
    
    // 初始化VrConfig模块
    IVrConfig::CreateInstance(&m_vrConfig);

    // 获取可执行文件路径
    QString exePath = QCoreApplication::applicationFilePath();

    // 获取可执行文件所在目录
    QString configPath = QFileInfo(exePath).absoluteDir().path() + "/config.xml";
    
    // 读取IP列表
    ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());

    // 初始化机械臂协议
    int 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
            // 初始化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);
            }
            m_pStatus->OnCamera1StatusChanged(camera1Connected);
            m_pStatus->OnStatusUpdate(camera1Connected ? "相机一连接成功" : "相机一连接失败");
            m_bCameraConnected = camera1Connected;
        }
        else {
            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("设备初始化完成");
    
    return SUCCESS;
}

// 初始化机械臂协议
int GrabBagPresenter::InitRobotProtocol()
{
    LOG_DEBUG("Start initializing robot protocol\n");
    
    // 创建RobotProtocol实例
    if(nullptr == m_pRobotProtocol){
        m_pRobotProtocol = new RobotProtocol();
    }
    
    // 设置连接状态回调
    m_pRobotProtocol->SetConnectionCallback([this](bool connected) {
        this->OnRobotConnectionChanged(connected);
    });

    // 设置工作信号回调
    m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork) {
        this->OnRobotWorkSignal(startWork);
    });
    
    // 初始化协议服务（使用端口503，避免与原有ModbusTCP冲突）
    int nRet = m_pRobotProtocol->Initialize();
    ERR_CODE_RETURN(nRet);

    LOG_INFO("Robot protocol initialization successful\n");
    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);
        m_pStatus->OnStatusUpdate(connected ? "机械臂已连接" : "机械臂连接断开");
    }
    
    // 检查并更新工作状态
    CheckAndUpdateWorkStatus();
}

void GrabBagPresenter::OnRobotWorkSignal(bool startWork)
{
    LOG_INFO("Received robot work signal: %s\n", (startWork ? "start work" : "stop work"));
    
    if (nullptr == m_pStatus) {
        return;
    }
    if (startWork) {
        m_pStatus->OnStatusUpdate("收到开始工作信号，启动检测");
        StartDetection();
    } else {
        m_pStatus->OnStatusUpdate("收到停止工作信号，停止检测");
        StopDetection();
    }
}

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

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

    // 通知UI工作状态变更为"正在工作"
    if (m_pStatus) {
        m_currentWorkStatus = WorkStatus::Working;
        m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
    }

    if(m_vrEyeDeviceList.empty())
    {
        LOG_ERROR("No camera device found\n");
        return ERR_CODE(DEV_NOT_FIND);
    }

    // 清空检测数据缓存
    {
        std::lock_guard<std::mutex> lock(m_detectionDataMutex);
        m_detectionDataCache.clear();
    }

    // 启动所有相机的检测
    for (size_t i = 0; i < m_vrEyeDeviceList.size(); ++i) {
        IVrEyeDevice* pDevice = m_vrEyeDeviceList[i];
        if (pDevice) {
            int ret = pDevice->StartDetect(&GrabBagPresenter::_StaticDetectionCallback, this);
            if (ret == 0) {
                LOG_INFO("Camera %zu start detection successfully\n", i + 1);
            } else {
                LOG_WARNING("Camera %zu start detection failed, error code: %d\n", i + 1, ret);
            }
        }
    }
    

    return 0;
}

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("检测已停止");
    }

    return 0;
}

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

// 静态回调函数实现
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, dataLength=%u\n", (int)eStatus, nDataLength);
    
    switch (eStatus) {
        case EVzDeviceWorkStatus::keDeviceWorkStatus_Offline:
        {
            LOG_WARNING("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 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("Received scan/trigger signal from camera\n");
            
            // 如果当前状态为就绪，通知检测线程开始处理
            if (m_currentWorkStatus == WorkStatus::Ready) {
                LOG_INFO("Notifying detection thread to start processing\n");
                
                // 通知算法检测线程
                {
                    std::lock_guard<std::mutex> lock(m_algoDetectMutex);
                    // 这里可以设置一个标志位，或者直接通知条件变量
                }
                m_algoDetectCondition.notify_one();
                
                if (m_pStatus) {
                    m_pStatus->OnStatusUpdate("收到扫描信号，开始检测处理");
                }
            } else {
                LOG_WARNING("Received scan signal but device not ready, current status: %d\n", (int)m_currentWorkStatus);
            }
            break;
        }
        
        default:
            LOG_DEBUG("Unhandled camera status: %d\n", (int)eStatus);
            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) {
        return;
    }

    // 将检测数据保存到缓存中
    {
        std::lock_guard<std::mutex> lock(m_detectionDataMutex);
        m_detectionDataCache.push_back(pLaserLinePoint);
        
        // LOG_DEBUG("Detection data cached: Frame %llu, Points: %d\n", 
        //          pLaserLinePoint->sLineData.llFrameIdx,
        //          pLaserLinePoint->sLineData.nPointCount);
    }
}

void GrabBagPresenter::CheckAndUpdateWorkStatus()
{
    if (m_bCameraConnected && m_bRobotConnected) {
        m_currentWorkStatus = WorkStatus::Ready;
        m_pStatus->OnWorkStatusChanged(WorkStatus::Ready);
        m_pStatus->OnStatusUpdate("设备已准备好");
    } else {
        m_currentWorkStatus = WorkStatus::Error;
        m_pStatus->OnWorkStatusChanged(WorkStatus::Error);
        m_pStatus->OnStatusUpdate("设备未准备好");
    }
}

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

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

int GrabBagPresenter::_DetectTask()
{   
    // 创建测试图像
    QImage image(":/resource/result_image.jpg");
    
    // 创建测试数据
    DetectionResult testResult;
    testResult.image = image;
    
    // 创建第一层数据
    GrabBagPositionLayout layer1;
    layer1.layerIndex = 1;
    
    GrabBagPosition pos1;
    pos1.x = 100.5;
    pos1.y = 200.3;
    pos1.z = 50.22;
    pos1.roll = 45.0;
    pos1.pitch = 10.0;
    pos1.yaw = 20.0;
    
    GrabBagPosition pos2;
    pos2.x = 110.8;
    pos2.y = 220.5;
    pos2.z = 52.1;
    pos2.roll = 48.5;
    pos2.pitch = 12.3;
    pos2.yaw = 22.7;
    
    layer1.position.push_back(pos1);
    layer1.position.push_back(pos2);
    
    // 创建第二层数据
    GrabBagPositionLayout layer2;
    layer2.layerIndex = 2;
    
    GrabBagPosition pos3;
    pos3.x = 150.2;
    pos3.y = 180.7;
    pos3.z = 55.0;
    pos3.roll = 30.0;
    pos3.pitch = 15.0;
    pos3.yaw = 25.0;
    
    layer2.position.push_back(pos3);
    
    // 添加层数据到结果
    testResult.positionLayout.push_back(layer1);
    testResult.positionLayout.push_back(layer2);
    
    // 调用检测结果回调函数
    m_pStatus->OnDetectionResult(testResult);
    
    // 更新状态
    m_pStatus->OnStatusUpdate("检测完成，发现3个目标");
    
    // 检测完成后，将工作状态更新为"完成"
    if (m_pStatus) {
        m_currentWorkStatus = WorkStatus::Completed;
        m_pStatus->OnWorkStatusChanged(WorkStatus::Completed);
    }
    
    // 更新机械臂协议状态
    if (m_pRobotProtocol) {
        
        // 更新检测结果坐标（以第一个检测到的目标为例）
        if (!testResult.positionLayout.empty() && !testResult.positionLayout[0].position.empty()) {
            const auto& firstPos = testResult.positionLayout[0].position[0];
            RobotProtocol::RobotCoordinate detectedCoord;
            detectedCoord.x = firstPos.x;
            detectedCoord.y = firstPos.y;
            detectedCoord.z = firstPos.z;
            detectedCoord.rx = firstPos.roll;
            detectedCoord.ry = firstPos.pitch;
            detectedCoord.rz = firstPos.yaw;
            m_pRobotProtocol->SetRobotCoordinate(detectedCoord);
            
            LOG_INFO("Update detection result coordinates: X=%.2f, Y=%.2f, Z=%.2f\n", 
                     detectedCoord.x, detectedCoord.y, detectedCoord.z);
        }
    }

    m_currentWorkStatus = WorkStatus::Ready;
    return SUCCESS;
}