GrabBag/App/WorkpieceProject/WorkpiecePositionApp/Presenter/Src/WorkpiecePositionPresenter.cpp

#include "VrError.h"
#include "WorkpiecePositionPresenter.h"
#include "VrLog.h"
#include <QDebug>
#include <QDateTime>

WorkpiecePositionPresenter::WorkpiecePositionPresenter(QObject *parent)
    : BasePresenter(parent)
    , m_pMarkReconnectTimer(new QTimer(this))
    , m_pEpicEyeReconnectTimer(new QTimer(this))
{
    connect(m_pMarkReconnectTimer, &QTimer::timeout, this, &WorkpiecePositionPresenter::onMarkReconnectTimeout);
    connect(m_pEpicEyeReconnectTimer, &QTimer::timeout, this, &WorkpiecePositionPresenter::onEpicEyeReconnectTimeout);
}

WorkpiecePositionPresenter::~WorkpiecePositionPresenter()
{
    // 停止定时器
    if (m_pMarkReconnectTimer) m_pMarkReconnectTimer->stop();
    if (m_pEpicEyeReconnectTimer) m_pEpicEyeReconnectTimer->stop();

    // 等待异步检测任务完成，防止访问已销毁的成员
    if (m_detectionFuture && m_detectionFuture->valid()) {
        LOG_DEBUG("等待检测任务完成...\n");
        m_detectionFuture->wait();
        LOG_DEBUG("检测任务已完成\n");
    }

    // 释放设备资源
    if (m_pMarkReceiver) { m_pMarkReceiver->Disconnect(); delete m_pMarkReceiver; }
    if (m_pEpicEyeDevice) { m_pEpicEyeDevice->Disconnect(); delete m_pEpicEyeDevice; }
    if (m_pConfig) delete m_pConfig;
}

// ============ 实现 BasePresenter 纯虚函数 ============

int WorkpiecePositionPresenter::InitApp()
{
    LOG_INFO("WorkpiecePositionPresenter InitApp\n");

    int ret = InitConfig();
    if (ret != SUCCESS) {
        LOG_ERR("初始化配置失败: %d\n", ret);
        return ret;
    }

    ret = InitBinocularMarkReceiver();
    if (ret != SUCCESS) {
        LOG_ERR("初始化BinocularMarkReceiver失败: %d\n", ret);
        m_pMarkReconnectTimer->start(5000);
    }

    ret = InitEpicEyeDevice();
    if (ret != SUCCESS) {
        LOG_ERR("初始化EpicEyeDevice失败: %d\n", ret);
        m_pEpicEyeReconnectTimer->start(5000);
    }

    LOG_INFO("WorkpiecePositionPresenter InitApp完成\n");
    return SUCCESS;
}

int WorkpiecePositionPresenter::InitAlgoParams()
{
    // 工件定位不需要额外的算法参数初始化
    return SUCCESS;
}

int WorkpiecePositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
{
    // 工件定位使用自己的检测流程，不使用BasePresenter的激光数据检测
    // 此方法不会被调用，因为我们重写了 StartDetection
    return SUCCESS;
}

EVzResultDataType WorkpiecePositionPresenter::GetDetectionDataType()
{
    // 工件定位不使用激光数据类型
    return keResultDataType_Position;
}

void WorkpiecePositionPresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
{
    // 使用自定义的设备状态通知
    auto pCallback = GetStatusCallback<IYWorkpiecePositionStatus>();
    if (!pCallback) return;

    if (cameraIndex == 1) {
        pCallback->OnBinocularMarkConnectionChanged(isConnected);
    } else if (cameraIndex == 2) {
        pCallback->OnEpicEyeConnectionChanged(isConnected);
    }
}

// ============ 重写 BasePresenter 虚函数 ============

void WorkpiecePositionPresenter::OnWorkStatusChanged(WorkStatus status)
{
    auto pCallback = GetStatusCallback<IYWorkpiecePositionStatus>();
    if (pCallback) {
        pCallback->OnWorkStatusChanged(status);
    }
}

void WorkpiecePositionPresenter::OnStatusUpdate(const std::string& statusMessage)
{
    auto pCallback = GetStatusCallback<IYWorkpiecePositionStatus>();
    if (pCallback) {
        pCallback->OnStatusUpdate(statusMessage);
    }
}

// ============ 重写检测方法 ============

int WorkpiecePositionPresenter::StartDetection(int cameraIndex, bool isAuto)
{
    std::lock_guard<std::mutex> lock(m_detectMutex);
    if (m_bDetecting) {
        LOG_WARN("正在检测中\n");
        return ERR_CODE(APP_ERR_EXEC);
    }
    if (!m_bMarkConnected) {
        LOG_ERR("BinocularMarkApp未连接\n");
        OnStatusUpdate("BinocularMarkApp未连接");
        return -2;
    }
    if (!m_bEpicEyeConnected) {
        LOG_ERR("EpicEye设备未连接\n");
        OnStatusUpdate("EpicEye设备未连接");
        return -3;
    }

    m_bDetecting = true;
    SetWorkStatus(WorkStatus::Working);
    LOG_INFO("开始检测\n");

    // 使用后台线程调用同步方法 RequestSingleDetection
    m_detectionFuture = std::make_shared<std::future<void>>(
        std::async(std::launch::async, [this]() {
            auto result = m_pMarkReceiver->RequestSingleDetection(5000);
            if (result.timestamp > 0) {
                OnMarkResult(result.marks, result.timestamp, result.errorCode);
            } else {
                LOG_ERR("BinocularMarkApp检测超时或失败\n");
                OnMarkResult({}, 0, -1);
            }
        })
    );

    return SUCCESS;
}

int WorkpiecePositionPresenter::StopDetection()
{
    std::lock_guard<std::mutex> lock(m_detectMutex);
    if (!m_bDetecting) return SUCCESS;
    m_bDetecting = false;
    SetWorkStatus(WorkStatus::Ready);
    LOG_INFO("停止检测\n");
    return SUCCESS;
}

// ============ 私有方法实现 ============

int WorkpiecePositionPresenter::InitConfig()
{
    if (!IVrConfig::CreateInstance((IVrConfig**)&m_pConfig)) {
        LOG_ERR("创建配置管理器失败\n");
        return ERR_CODE(APP_ERR_EXEC);
    }

    std::string configPath = "./config/config.xml";
    int ret = m_pConfig->LoadConfig(configPath, m_configResult);
    if (ret != SUCCESS) {
        LOG_ERR("加载配置文件失败: %s\n", configPath.c_str());
        return -2;
    }

    // 从 binocularMarkConfig 获取 BinocularMark 连接信息
    m_binocularMarkIp = m_configResult.binocularMarkConfig.serverIP;
    m_binocularMarkPort = m_configResult.binocularMarkConfig.serverPort;

    // 从 epicEyeDeviceList 获取 EpicEye 设备信息
    if (m_configResult.epicEyeDeviceList.size() > 0) {
        m_epicEyeIp = m_configResult.epicEyeDeviceList[0].ip;
    } else {
        LOG_WARN("配置文件中未找到EpicEye设备，使用默认IP: 192.168.1.100\n");
        m_epicEyeIp = "192.168.1.100";
    }

    return SUCCESS;
}

int WorkpiecePositionPresenter::InitBinocularMarkReceiver()
{
    int ret = IBinocularMarkReceiver::CreateInstance(&m_pMarkReceiver);
    if (ret != SUCCESS) {
        LOG_ERR("创建BinocularMarkReceiver实例失败\n");
        return ERR_CODE(APP_ERR_EXEC);
    }

    m_pMarkReceiver->SetMarkResultCallback([this](const std::vector<VrMark3D>& marks, int64_t timestamp, int errorCode) {
        this->OnMarkResult(marks, timestamp, errorCode);
    });
    m_pMarkReceiver->SetEventCallback([this](ReceiverEventType eventType, const std::string& errorMsg) {
        this->OnMarkConnectionChanged(eventType == ReceiverEventType::CONNECTED);
    });

    return ConnectToBinocularMark();
}

int WorkpiecePositionPresenter::InitEpicEyeDevice()
{
    int ret = IEpicEyeDevice::CreateObject(&m_pEpicEyeDevice);
    if (ret != SUCCESS) {
        LOG_ERR("创建EpicEyeDevice实例失败\n");
        return ERR_CODE(APP_ERR_EXEC);
    }
    return ConnectToEpicEye();
}

void WorkpiecePositionPresenter::OnMarkResult(const std::vector<VrMark3D>& marks, qint64 timestamp, int errorCode)
{
    LOG_INFO("收到BinocularMark检测结果，Mark数量: %zu, 错误码: %d\n", marks.size(), errorCode);

    if (errorCode != 0) {
        LOG_ERR("BinocularMark检测失败: %d\n", errorCode);
        SetWorkStatus(WorkStatus::Error);
        OnStatusUpdate("BinocularMark检测失败");
        m_bDetecting = false;
        return;
    }

    { std::lock_guard<std::mutex> lock(m_detectMutex); m_lastMarks = marks; }

    PointCloudData pointCloud;
    int ret = m_pEpicEyeDevice->CapturePointCloud(pointCloud, 5000);
    if (ret != SUCCESS) {
        LOG_ERR("获取EpicEye点云数据失败: %d\n", ret);
        SetWorkStatus(WorkStatus::Error);
        OnStatusUpdate("获取EpicEye点云数据失败");
        m_bDetecting = false;
        return;
    }

    WorkpieceCenterPosition centerPosition;
    ret = CalculateWorkpieceCenter(marks, pointCloud, centerPosition);
    if (ret != SUCCESS) {
        LOG_ERR("计算工件中心点失败: %d\n", ret);
        SetWorkStatus(WorkStatus::Error);
        OnStatusUpdate("计算工件中心点失败");
        m_bDetecting = false;
        if (pointCloud.pData) delete[] pointCloud.pData;
        return;
    }

    if (pointCloud.pData) delete[] pointCloud.pData;

    WorkpiecePositionDetectionResult result;
    result.positions.push_back(centerPosition);
    result.marks = marks;

    auto pCallback = GetStatusCallback<IYWorkpiecePositionStatus>();
    if (pCallback) pCallback->OnDetectionResult(result);

    SetWorkStatus(WorkStatus::Completed);
    LOG_INFO("工件定位检测完成，中心点: (%.2f, %.2f, %.2f)\n", centerPosition.x, centerPosition.y, centerPosition.z);
    m_bDetecting = false;
}

void WorkpiecePositionPresenter::OnMarkConnectionChanged(bool connected)
{
    LOG_INFO("BinocularMark连接状态改变: %s\n", connected ? "已连接" : "断开连接");
    m_bMarkConnected = connected;

    auto pCallback = GetStatusCallback<IYWorkpiecePositionStatus>();
    if (pCallback) pCallback->OnBinocularMarkConnectionChanged(connected);

    if (connected) {
        m_pMarkReconnectTimer->stop();
        OnStatusUpdate("BinocularMarkApp已连接");
    } else {
        m_pMarkReconnectTimer->start(5000);
        OnStatusUpdate("BinocularMarkApp连接断开，正在重连...");
    }
}

int WorkpiecePositionPresenter::CalculateWorkpieceCenter(const std::vector<VrMark3D>& marks, const PointCloudData& pointCloud, WorkpieceCenterPosition& centerPosition)
{
    // TODO: 实现工件中心点计算算法（由用户后续填充）
    // 示例实现：简单地将所有Mark点的平均值作为中心点
    if (marks.empty()) {
        LOG_ERR("Mark数据为空\n");
        return ERR_CODE(APP_ERR_EXEC);
    }

    double sumX = 0.0, sumY = 0.0, sumZ = 0.0;
    for (const auto& mark : marks) {
        sumX += mark.x;
        sumY += mark.y;
        sumZ += mark.z;
    }

    centerPosition.x = sumX / marks.size();
    centerPosition.y = sumY / marks.size();
    centerPosition.z = sumZ / marks.size();
    centerPosition.roll = 0.0;
    centerPosition.pitch = 0.0;
    centerPosition.yaw = 0.0;

    LOG_INFO("计算得到工件中心点: (%.2f, %.2f, %.2f)\n", centerPosition.x, centerPosition.y, centerPosition.z);
    return SUCCESS;
}

int WorkpiecePositionPresenter::ConnectToBinocularMark()
{
    LOG_INFO("连接BinocularMarkApp: %s:%d\n", m_binocularMarkIp.c_str(), m_binocularMarkPort);
    return m_pMarkReceiver->Connect(m_binocularMarkIp, m_binocularMarkPort);
}

int WorkpiecePositionPresenter::ConnectToEpicEye()
{
    LOG_INFO("连接EpicEye设备: %s\n", m_epicEyeIp.c_str());
    int ret = m_pEpicEyeDevice->Connect(m_epicEyeIp);
    if (ret != SUCCESS) {
        LOG_ERR("连接EpicEye设备失败\n");
        m_bEpicEyeConnected = false;
        auto pCallback = GetStatusCallback<IYWorkpiecePositionStatus>();
        if (pCallback) pCallback->OnEpicEyeConnectionChanged(false);
        return ERR_CODE(APP_ERR_EXEC);
    }

    m_bEpicEyeConnected = true;
    auto pCallback = GetStatusCallback<IYWorkpiecePositionStatus>();
    if (pCallback) pCallback->OnEpicEyeConnectionChanged(true);
    m_pEpicEyeReconnectTimer->stop();
    OnStatusUpdate("EpicEye设备已连接");
    return SUCCESS;
}

void WorkpiecePositionPresenter::onMarkReconnectTimeout()
{
    LOG_INFO("尝试重连BinocularMarkApp\n");
    ConnectToBinocularMark();
}

void WorkpiecePositionPresenter::onEpicEyeReconnectTimeout()
{
    LOG_INFO("尝试重连EpicEye设备\n");
    ConnectToEpicEye();
}