GrabBag/BeltTearingServer/BeltTearingPresenter.cpp

#include "BeltTearingPresenter.h"
#include "PathManager.h"
#include "version.h"
#include <QUuid>
#include <QDataStream>
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <QDateTime>
#include <iostream>

#include "VrLog.h"

BeltTearingPresenter::BeltTearingPresenter(QObject *parent)
    : QObject(parent)
    , m_tcpServer(new QTcpServer(this))
    , m_cameraInitTimer(new QTimer(this))
    , m_port(0)
    , m_config(nullptr)
    , m_eyeDevice(nullptr)
    , m_beltTearingAlgo(new BeltTearingAlgo())
    , m_cameraInitialized(false)
    , m_cameraDetecting(false)
{
    // 打印版本信息
    LOG_INFO("Initializing %s %s\n", getProductName().toStdString().c_str(), getVersionString().toStdString().c_str());
    LOG_INFO("Build: %s\n", getBuildInfo().toStdString().c_str());
    
    // 连接信号槽
    connect(m_tcpServer, &QTcpServer::newConnection, this, &BeltTearingPresenter::onNewConnection);
    connect(m_cameraInitTimer, &QTimer::timeout, this, &BeltTearingPresenter::onCameraInitTimer);
    
    // 创建配置实例
    IVrBeltTearingConfig::CreateInstance(&m_config);
    if (m_config) {
        m_config->SetConfigChangeNotify(this);
    }
    
    // 初始化相机
    initializeCamera();
}

BeltTearingPresenter::~BeltTearingPresenter()
{
    stopServer();
    stopCamera();
    
    if (m_beltTearingAlgo) {
        delete m_beltTearingAlgo;
        m_beltTearingAlgo = nullptr;
    }
    
    if (m_config) {
        delete m_config;
        m_config = nullptr;
    }
}

bool BeltTearingPresenter::loadConfiguration(const QString& configFilePath)
{
    if (!m_config) {
        LOG_WARNING("Config instance not created\n");
        return false;
    }
    
    m_configFilePath = configFilePath;
    
    try {
        // 加载配置文件
        m_configResult = m_config->LoadConfig(configFilePath.toStdString());
        
        LOG_INFO("Configuration loaded successfully:\n");
        LOG_INFO("  Server Port: %d\n", m_configResult.serverPort);
        LOG_INFO("  Project Type: %d\n", static_cast<int>(m_configResult.projectType));
        LOG_INFO("  Servers count: %d\n", m_configResult.servers.size());
        
        // 应用算法参数
        applyAlgorithmParameters(m_configResult.algorithmParams);
        
        // 输出算法参数信息
        const auto& params = m_configResult.algorithmParams;
        LOG_INFO("Algorithm Parameters:\n");
        LOG_INFO("  Tear Threshold: %f\n", params.beltTearingParam.tearThreshold);
        LOG_INFO("  Min Tear Length: %f\n", params.beltTearingParam.minTearLength);
        LOG_INFO("  Max Tear Width: %f\n", params.beltTearingParam.maxTearWidth);
        LOG_INFO("  Blur Size: %d\n", params.imageProcessingParam.blurSize);
        LOG_INFO("  Check Interval: %d\n", params.monitoringParam.checkInterval);
        
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Error loading configuration: %s\n", e.what());
        return false;
    }
}

void BeltTearingPresenter::applyAlgorithmParameters(const BeltTearingAlgorithmParams& params)
{
    if (!m_beltTearingAlgo) {
        return;
    }
    
    // 重置算法参数
    m_beltTearingAlgo->ResetParameter();
    
    // 应用算法参数
    LOG_DEBUG("Applying algorithm parameters...\n");
    LOG_DEBUG("  Tear Threshold: %f\n", params.beltTearingParam.tearThreshold);
    LOG_DEBUG("  Min Tear Length: %f\n", params.beltTearingParam.minTearLength);
    LOG_DEBUG("  Max Tear Width: %f\n", params.beltTearingParam.maxTearWidth);
    LOG_DEBUG("  Blur Size: %d\n", params.imageProcessingParam.blurSize);
    LOG_DEBUG("  Canny Threshold 1: %f\n", params.imageProcessingParam.cannyThreshold1);
    LOG_DEBUG("  Canny Threshold 2: %f\n", params.imageProcessingParam.cannyThreshold2);
    LOG_DEBUG("  Check Interval: %d ms\n", params.monitoringParam.checkInterval);
    LOG_DEBUG("  Alert Threshold: %f\n", params.monitoringParam.alertThreshold);
    
    // 调试参数
    if (m_configResult.debugParam.enableDebug) {
        LOG_DEBUG("Debug mode enabled:\n");
        LOG_DEBUG("  Save debug images: %s\n", (m_configResult.debugParam.saveDebugImage ? "Yes" : "No"));
        LOG_DEBUG("  Print detail log: %s\n", (m_configResult.debugParam.printDetailLog ? "Yes" : "No"));
    }
    
    // TODO: 根据BeltTearingAlgo的实际接口设置参数
    // 例如：m_beltTearingAlgo->SetTearThreshold(params.beltTearingParam.tearThreshold);
}

void BeltTearingPresenter::OnConfigChanged(const BeltTearingConfigResult& configResult)
{
    LOG_INFO("Configuration changed notification received\n");
    
    // 更新配置结果
    m_configResult = configResult;
    
    // 重新应用算法参数
    applyAlgorithmParameters(configResult.algorithmParams);
    
    // 如果服务器端口改变，可能需要重启服务器
    if (m_tcpServer->isListening() && m_tcpServer->serverPort() != configResult.serverPort) {
        LOG_INFO("Server port changed, restarting server...\n");
        stopServer();
        startServer(configResult.serverPort);
    }
}

bool BeltTearingPresenter::initializeCamera()
{
    if (m_eyeDevice) {
        return true; // 已经初始化过
    }
    
    int result = IVrEyeDevice::CreateObject(&m_eyeDevice);
    if (result != 0 || !m_eyeDevice) {
        LOG_ERROR("Failed to create VrEyeDevice object, error code: %d\n", result);
        return false;
    }
    
    result = m_eyeDevice->InitDevice();
    if (result != 0) {
        LOG_ERROR("Failed to initialize VrEyeDevice, error code: %d\n", result);
        delete m_eyeDevice;
        m_eyeDevice = nullptr;
        return false;
    }
    
    // 设置状态回调
    result = m_eyeDevice->SetStatusCallback(OnStatusCallback, this);
    if (result != 0) {
        LOG_WARNING("Failed to set status callback, error code: %d\n", result);
    }
    
    LOG_INFO("VrEyeDevice initialized successfully\n");
    return true;
}

void BeltTearingPresenter::startCamera()
{
    if (!m_eyeDevice) {
        if (!initializeCamera()) {
            // 启动定时器持续重试初始化
            LOG_WARNING("Camera initialization failed, starting retry timer...\n");
            m_cameraInitTimer->start(5000); // 每5秒重试一次
            return;
        }
    }
    
    // 尝试打开设备
    int result = m_eyeDevice->OpenDevice(nullptr);
    if (result != 0) {
        LOG_WARNING("Failed to open camera device, error code: %d, retrying...\n", result);
        
        // 启动定时器持续重试连接
        m_cameraInitTimer->start(3000); // 每3秒重试一次
        return;
    }
    
    // 停止重试定时器
    m_cameraInitTimer->stop();
    m_cameraInitialized = true;
    
    // 开始检测
    result = m_eyeDevice->StartDetect(OnPointCloudCallback, keResultDataType_Position, this);
    if (result != 0) {
        LOG_ERROR("Failed to start detection, error code: %d\n", result);
        return;
    }
    
    m_cameraDetecting = true;
}

void BeltTearingPresenter::stopCamera()
{
    m_cameraInitTimer->stop();
    
    if (m_eyeDevice) {
        if (m_cameraDetecting) {
            m_eyeDevice->StopDetect();
            m_cameraDetecting = false;
        }
        
        if (m_cameraInitialized) {
            m_eyeDevice->CloseDevice();
            m_cameraInitialized = false;
        }
        
        delete m_eyeDevice;
        m_eyeDevice = nullptr;
    }
    
    LOG_INFO("Camera stopped\n");
}

void BeltTearingPresenter::onCameraInitTimer()
{
    LOG_DEBUG("Retrying camera initialization...\n");
    
    // 尝试重新初始化和连接相机
    if (!m_eyeDevice) {
        if (!initializeCamera()) {
            return; // 继续重试
        }
    }
    
    // 尝试连接相机
    int result = m_eyeDevice->OpenDevice(nullptr);
    if (result == 0) {
        // 连接成功，开始检测
        m_cameraInitTimer->stop();
        m_cameraInitialized = true;
        
        result = m_eyeDevice->StartDetect(OnPointCloudCallback, keResultDataType_Position, this);
        if (result == 0) {
            m_cameraDetecting = true;
            LOG_INFO("Camera connection restored successfully!\n");
        } else {
            LOG_ERROR("Failed to start detection after reconnection, error code: %d\n", result);
        }
    } else {
        LOG_WARNING("Still unable to connect to camera, continuing retry...\n");
    }
}

void BeltTearingPresenter::OnStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
    BeltTearingPresenter* presenter = static_cast<BeltTearingPresenter*>(pInfoParam);
    if (!presenter) return;
    
    LOG_DEBUG("Camera status changed: %d\n", static_cast<int>(eStatus));
    
    // 处理相机状态变化
    switch (eStatus) {
        case keDeviceWorkStatus_Eye_Comming:
            LOG_INFO("Camera connected\n");
            break;
        case keDeviceWorkStatus_Offline:
            LOG_WARNING("Camera disconnected, attempting to reconnect...\n");
            presenter->m_cameraInitialized = false;
            presenter->m_cameraDetecting = false;
            // 开始重连尝试
            if (!presenter->m_cameraInitTimer->isActive()) {
                presenter->m_cameraInitTimer->start(3000);
            }
            break;
        default:
            break;
    }
}

void BeltTearingPresenter::OnPointCloudCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pParam)
{
    if(keResultDataType_Position != eDataType) return;

    BeltTearingPresenter* presenter = static_cast<BeltTearingPresenter*>(pParam);
    if (!presenter || !pLaserLinePoint) return;
    
    // 处理点云数据
    presenter->processPointCloudData(pLaserLinePoint);
}

void BeltTearingPresenter::processPointCloudData(const SVzLaserLineData* pLaserLine)
{
    if (!m_beltTearingAlgo || !pLaserLine) return;

    try {

        SVzNL3DLaserLine sNL3DLaserLine;
        sNL3DLaserLine.nTimeStamp = pLaserLine->llTimeStamp;
        sNL3DLaserLine.p3DPosition = static_cast<SVzNL3DPosition*>(pLaserLine->p3DPoint);
        sNL3DLaserLine.nPositionCnt = pLaserLine->nPointCount;
        // 调用算法进行检测
        std::vector<SSG_beltTearingInfo> results = m_beltTearingAlgo->Predit(&sNL3DLaserLine, pLaserLine->llFrameIdx);
        
        // 如果有检测结果，发送到网络客户端
        if (!results.empty()) {
            LOG_INFO("Detected %d belt tearing(s)\n", results.size());
            sendTearingResults(results);
        }
        
    } catch (const std::exception& e) {
        LOG_ERROR("Error in algorithm processing: %s\n", e.what());
    }
}

void BeltTearingPresenter::sendTearingResults(const std::vector<SSG_beltTearingInfo>& results)
{
    if (results.empty() || m_clients.isEmpty()) {
        return;
    }
    
    // 将检测结果转换为JSON格式
    QJsonArray tearingArray;
    
    for (const auto& result : results) {
        QJsonObject tearingObj;
        // tearingObj["level"]         = QString::number(result.level);
        tearingObj["id"]            = QString::number(result.tearID);
        tearingObj["tearStatus"]    = QString::number(static_cast<int>(result.tearStatus));
        tearingObj["tearType"]      = QString::number(result.tearType);
        tearingObj["statLineIdx"]   = QString::number(result.statLineIdx);
        tearingObj["endLineIdx"]    = QString::number(result.endLineIdx);
        tearingObj["tearDepth"]     = QString::number(result.tearDepth);
        tearingObj["tearWidth"]     = QString::number(result.tearWidth);
        // tearingObj["tearLength"]    = QString::number(result.tearLength);
        tearingObj["roiLeft"]       = QString::number(result.roi.left);
        tearingObj["roiRight"]      = QString::number(result.roi.right);
        tearingObj["roiTop"]        = QString::number(result.roi.top);
        tearingObj["roiBottom"]     = QString::number(result.roi.bottom);
        // tearingObj["imageFile"]     = QString::fromStdString(result.imageFile);
        // tearingObj["older"]         = QString::fromStdString(result.older);
        tearingObj["timestamp"]     = QDateTime::currentDateTime().toString(Qt::ISODate);
        
        tearingArray.append(tearingObj);
    }
    
    // 转换为JSON字符串
    QJsonDocument doc(tearingArray);
    QByteArray message = doc.toJson(QJsonDocument::Compact);
    
    // 创建数据包
    QByteArray package;
    QDataStream stream(&package, QIODevice::WriteOnly);
    stream.setByteOrder(QDataStream::BigEndian);
    stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(message.size());
    package.append(message);
    package.append("___END___\r\n");
    
    // 发送到所有连接的客户端
    for (auto it = m_clients.begin(); it != m_clients.end(); ++it) {
        QTcpSocket *socket = it.value();
        if (socket->state() == QAbstractSocket::ConnectedState) {
            socket->write(package);
        }
    }
    
    LOG_INFO("Sent tearing detection results to %d client(s)\n", m_clients.size());
}

bool BeltTearingPresenter::startServer(quint16 port)
{
    if (m_tcpServer->isListening()) {
        stopServer();
    }
    
    m_port = port;
    bool result = m_tcpServer->listen(QHostAddress::Any, port);
    
    if (result) {
        LOG_INFO("TCP server started on port %d\n", port);
    } else {
        LOG_ERROR("Failed to start TCP server: %s\n", m_tcpServer->errorString().toStdString().c_str());
    }
    
    return result;
}

void BeltTearingPresenter::stopServer()
{
    // 断开所有客户端连接
    for (auto it = m_clients.begin(); it != m_clients.end(); ++it) {
        it.value()->disconnectFromHost();
    }
    m_clients.clear();
    
    // 停止服务器监听
    if (m_tcpServer->isListening()) {
        m_tcpServer->close();
    }
    
    m_port = 0;
}

void BeltTearingPresenter::onNewConnection()
{
    while (m_tcpServer->hasPendingConnections()) {
        QTcpSocket *socket = m_tcpServer->nextPendingConnection();
        
        // 生成客户端ID
        QString clientId = generateClientId(socket);
        
        // 存储客户端连接
        m_clients[clientId] = socket;
        
        // 连接客户端信号
        connect(socket, &QTcpSocket::disconnected, this, &BeltTearingPresenter::onClientDisconnected);
        
        LOG_INFO("Client connected: %s\n", clientId.toStdString().c_str());
        
        // 发送欢迎消息
        QByteArray welcomeMsg = "Welcome to BeltTearing Server!";
        socket->write(welcomeMsg);
    }
}

void BeltTearingPresenter::onClientDisconnected()
{
    QTcpSocket *socket = qobject_cast<QTcpSocket*>(sender());
    if (!socket) {
        return;
    }
    
    // 查找断开连接的客户端ID
    QString clientId;
    for (auto it = m_clients.begin(); it != m_clients.end(); ++it) {
        if (it.value() == socket) {
            clientId = it.key();
            break;
        }
    }
    
    if (!clientId.isEmpty()) {
        m_clients.remove(clientId);
        LOG_INFO("Client disconnected: %s\n", clientId.toStdString().c_str());
    }
    
    socket->deleteLater();
}

QString BeltTearingPresenter::generateClientId(QTcpSocket *socket)
{
    // 使用UUID生成唯一的客户端ID
    return QUuid::createUuid().toString();
}

QString BeltTearingPresenter::getVersionString()
{
    return QString(BELT_TEARING_SERVER_VERSION_STRING);
}

QString BeltTearingPresenter::getProductName()
{
    return QString(BELT_TEARING_SERVER_PRODUCT_NAME);
}

QString BeltTearingPresenter::getBuildInfo()
{
    return QString("%1 %2 built on %3 %4 for %5")
            .arg(BELT_TEARING_SERVER_BUILD_TYPE)
            .arg(BELT_TEARING_SERVER_VERSION_STRING)
            .arg(BELT_TEARING_SERVER_BUILD_DATE)
            .arg(BELT_TEARING_SERVER_BUILD_TIME)
            .arg(BELT_TEARING_SERVER_PLATFORM);
}