GrabBag/App/BinocularMark/BinocularMarkApp/BinocularMarkTcpProtocol.cpp

#include "BinocularMarkTcpProtocol.h"
#include "VrLog.h"
#include <QDateTime>
#include <QBuffer>
#include <cstring>

// 静态实例指针
BinocularMarkTcpProtocol* BinocularMarkTcpProtocol::s_pInstance = nullptr;

BinocularMarkTcpProtocol::BinocularMarkTcpProtocol(QObject *parent)
    : QObject(parent)
    , m_pTcpServer(nullptr)
    , m_pHeartbeatTimer(nullptr)
    , m_nHeartbeatInterval(30)
    , m_nTcpPort(5901)
{
    s_pInstance = this;

    // 创建TCP服务器
    VrCreatYTCPServer(&m_pTcpServer);

    // 创建心跳定时器
    m_pHeartbeatTimer = new QTimer(this);
    connect(m_pHeartbeatTimer, &QTimer::timeout, this, &BinocularMarkTcpProtocol::onHeartbeatTimeout);

    LOG_INFO("BinocularMarkTcpProtocol created\n");
}

BinocularMarkTcpProtocol::~BinocularMarkTcpProtocol()
{
    stopHeartbeat();
    stopServer();

    if (m_pTcpServer != nullptr)
    {
        delete m_pTcpServer;
        m_pTcpServer = nullptr;
    }

    s_pInstance = nullptr;
    LOG_INFO("BinocularMarkTcpProtocol destroyed\n");
}

bool BinocularMarkTcpProtocol::startServer(quint16 port)
{
    if (m_pTcpServer == nullptr)
    {
        LOG_ERROR("TCP server not created\n");
        return false;
    }

    m_nTcpPort = port;

    // 初始化TCP服务器
    if (!m_pTcpServer->Init(port, false))
    {
        LOG_ERROR("Failed to init TCP server on port %d\n", port);
        return false;
    }

    // 设置事件回调
    m_pTcpServer->SetEventCallback(tcpEventCallback);

    // 启动TCP服务器
    if (!m_pTcpServer->Start(tcpRecvCallback, false))
    {
        LOG_ERROR("Failed to start TCP server\n");
        return false;
    }

    LOG_INFO("TCP server started on port %d\n", port);
    return true;
}

void BinocularMarkTcpProtocol::stopServer()
{
    if (m_pTcpServer != nullptr)
    {
        m_pTcpServer->Stop();
        m_pTcpServer->Close();
        LOG_INFO("TCP server stopped\n");
    }

    // 清空客户端缓冲区
    m_clientBuffers.clear();
}

void BinocularMarkTcpProtocol::startHeartbeat(int heartbeatInterval)
{
    m_nHeartbeatInterval = heartbeatInterval;
    m_pHeartbeatTimer->start(heartbeatInterval * 1000);
    LOG_INFO("Heartbeat started, interval: %d seconds\n", heartbeatInterval);
}

void BinocularMarkTcpProtocol::stopHeartbeat()
{
    if (m_pHeartbeatTimer != nullptr)
    {
        m_pHeartbeatTimer->stop();
    }
    LOG_INFO("Heartbeat stopped\n");
}

void BinocularMarkTcpProtocol::onHeartbeatTimeout()
{
    // 发送心跳消息给所有客户端
    QJsonObject heartbeatObj;
    heartbeatObj["msg_type"] = "heartbeat";
    heartbeatObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();

    QJsonDocument doc(heartbeatObj);
    QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
    QByteArray frameData = buildFrame(jsonData);

    if (m_pTcpServer != nullptr)
    {
        m_pTcpServer->SendAllData(frameData.data(), frameData.size());
    }
}

void BinocularMarkTcpProtocol::tcpRecvCallback(const TCPClient* pClient, const char* pData, const unsigned int nLen)
{
    if (s_pInstance != nullptr)
    {
        s_pInstance->handleReceivedData(pClient, pData, nLen);
    }
}

void BinocularMarkTcpProtocol::tcpEventCallback(const TCPClient* pClient, TCPServerEventType eventType)
{
    if (s_pInstance == nullptr)
        return;

    QString clientId = s_pInstance->generateClientId(pClient);

    switch (eventType)
    {
    case TCP_EVENT_CLIENT_CONNECTED:
        LOG_INFO("Client connected: %s\n", clientId.toStdString().c_str());
        s_pInstance->m_clientBuffers[clientId].clear();
        break;

    case TCP_EVENT_CLIENT_DISCONNECTED:
        LOG_INFO("Client disconnected: %s\n", clientId.toStdString().c_str());
        s_pInstance->m_clientBuffers.remove(clientId);
        break;

    case TCP_EVENT_CLIENT_EXCEPTION:
        LOG_WARN("Client exception: %s\n", clientId.toStdString().c_str());
        s_pInstance->m_clientBuffers.remove(clientId);
        break;

    default:
        break;
    }
}

void BinocularMarkTcpProtocol::handleReceivedData(const TCPClient* pClient, const char* pData, unsigned int nLen)
{
    QString clientId = generateClientId(pClient);

    // 将接收到的数据追加到客户端缓冲区
    QByteArray receivedData(pData, nLen);
    m_clientBuffers[clientId].append(receivedData);

    // 解析数据帧（处理粘包）
    QList<QByteArray> jsonDataList;
    int frameCount = parseFrames(clientId, m_clientBuffers[clientId], jsonDataList);

    // 处理每个完整的JSON消息
    for (const QByteArray& jsonData : jsonDataList)
    {
        handleJsonMessage(pClient, jsonData);
    }

    LOG_DEBUG("Received %u bytes from %s, parsed %d frames\n",
              nLen, clientId.toStdString().c_str(), frameCount);
}

QByteArray BinocularMarkTcpProtocol::buildFrame(const QByteArray& jsonData)
{
    QByteArray frame;

    // 帧头（8字节）
    frame.append(FRAME_HEADER, FRAME_HEADER_SIZE);

    // 写入数据长度（8位字符串格式）
    quint64 dataLength = jsonData.size();
    char lengthStr[9];  // 8位数字 + '\0'
#ifdef _WIN32
    sprintf_s(lengthStr, "%08u", dataLength);
#else
    sprintf(lengthStr, "%08u", dataLength);
#endif
    frame.append(lengthStr, FRAME_LENGTH_SIZE);

    // JSON数据
    frame.append(jsonData);

    // 帧尾（4字节）
    frame.append(FRAME_TAIL, FRAME_TAIL_SIZE);

    return frame;
}

int BinocularMarkTcpProtocol::parseFrames(const QString& clientId, const QByteArray& data, QList<QByteArray>& outJsonData)
{
    QByteArray& buffer = m_clientBuffers[clientId];
    int frameCount = 0;

    while (true)
    {
        // 检查缓冲区是否有足够数据（至少包含帧头+长度字段）
        if (buffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE)
            break;

        // 查找帧头
        int headerPos = buffer.indexOf(FRAME_HEADER, 0);
        if (headerPos < 0)
        {
            // 没有找到帧头，清空缓冲区
            buffer.clear();
            break;
        }

        // 如果帧头不在起始位置，丢弃之前的数据
        if (headerPos > 0)
        {
            buffer.remove(0, headerPos);
        }

        // 检查是否有完整的长度字段
        if (buffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE)
            break;

        // 读取数据长度（8字节ASCII字符串格式，如 "00001234"）
        QByteArray lengthStr = buffer.mid(FRAME_HEADER_SIZE, FRAME_LENGTH_SIZE);
        bool ok = false;
        quint64 dataLength = lengthStr.toULongLong(&ok);

        if (!ok)
        {
            LOG_ERROR("Invalid length string: %s\n", lengthStr.toStdString().c_str());
            buffer.remove(0, FRAME_HEADER_SIZE);
            continue;
        }

        // 计算完整帧的长度
        quint64 frameLength = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE;

        // 检查是否接收到完整的帧
        if (buffer.size() < static_cast<int>(frameLength))
            break;

        // 检查帧尾
        int tailPos = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength;
        if (buffer.mid(tailPos, FRAME_TAIL_SIZE) != QByteArray(FRAME_TAIL, FRAME_TAIL_SIZE))
        {
            // 帧尾不匹配，丢弃当前帧头，继续查找下一个帧头
            buffer.remove(0, FRAME_HEADER_SIZE);
            continue;
        }

        // 提取JSON数据
        QByteArray jsonData = buffer.mid(FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE, dataLength);
        outJsonData.append(jsonData);

        // 从缓冲区移除已处理的帧
        buffer.remove(0, frameLength);

        frameCount++;
    }

    return frameCount;
}

MarkMessageType BinocularMarkTcpProtocol::parseMessageType(const QString& msgTypeStr)
{
    if (msgTypeStr == "mark_result")
        return MarkMessageType::MARK_RESULT;
    else if (msgTypeStr == "heartbeat")
        return MarkMessageType::HEARTBEAT;
    else if (msgTypeStr == "heartbeat_ack")
        return MarkMessageType::HEARTBEAT_ACK;
    else if (msgTypeStr == "trigger")
        return MarkMessageType::CMD_TRIGGER;
    else if (msgTypeStr == "cmd_response")
        return MarkMessageType::CMD_RESPONSE;
    else
        return MarkMessageType::UNKNOWN;
}

void BinocularMarkTcpProtocol::handleJsonMessage(const TCPClient* pClient, const QByteArray& jsonData)
{
    QJsonParseError parseError;
    QJsonDocument doc = QJsonDocument::fromJson(jsonData, &parseError);

    if (parseError.error != QJsonParseError::NoError)
    {
        LOG_ERROR("Failed to parse JSON: %s\n", parseError.errorString().toStdString().c_str());
        return;
    }

    if (!doc.isObject())
    {
        LOG_ERROR("JSON is not an object\n");
        return;
    }

    QJsonObject jsonObj = doc.object();

    // 解析消息类型
    QString msgTypeStr = jsonObj["msg_type"].toString();
    MarkMessageType msgType = parseMessageType(msgTypeStr);

    // 根据消息类型处理
    switch (msgType)
    {
    case MarkMessageType::HEARTBEAT:
        handleHeartbeat(pClient, jsonObj);
        break;

    case MarkMessageType::CMD_TRIGGER:
        handleTriggerCommand(pClient, jsonObj);
        break;

    case MarkMessageType::HEARTBEAT_ACK:
        // 心跳应答，不做处理
        break;

    default:
        LOG_WARN("Unknown message type: %s\n", msgTypeStr.toStdString().c_str());
        break;
    }
}

void BinocularMarkTcpProtocol::handleHeartbeat(const TCPClient* pClient, const QJsonObject& jsonObj)
{
    // 发送心跳应答
    sendHeartbeatAck(pClient);
}

void BinocularMarkTcpProtocol::handleTriggerCommand(const TCPClient* pClient, const QJsonObject& jsonObj)
{
    // 触发检测
    emit triggerDetection();

    // 发送命令应答
    sendCommandResponse(pClient, "trigger", true, 0, "OK");
}

void BinocularMarkTcpProtocol::sendHeartbeatAck(const TCPClient* pClient)
{
    QJsonObject ackObj;
    ackObj["msg_type"] = "heartbeat_ack";
    ackObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();

    QJsonDocument doc(ackObj);
    QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
    QByteArray frameData = buildFrame(jsonData);

    if (m_pTcpServer != nullptr)
    {
        m_pTcpServer->SendData(pClient, frameData.data(), frameData.size());
    }
}

void BinocularMarkTcpProtocol::sendCommandResponse(const TCPClient* pClient, const QString& cmdType,
                                                    bool result, int errorCode, const QString& errorMsg)
{
    QJsonObject responseObj;
    responseObj["msg_type"] = "cmd_response";
    responseObj["cmd_type"] = cmdType;
    responseObj["result"] = result;
    responseObj["error_code"] = errorCode;
    responseObj["error_msg"] = errorMsg;
    responseObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();

    QJsonDocument doc(responseObj);
    QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
    QByteArray frameData = buildFrame(jsonData);

    if (m_pTcpServer != nullptr)
    {
        m_pTcpServer->SendData(pClient, frameData.data(), frameData.size());
    }
}

void BinocularMarkTcpProtocol::sendMarkResult(const std::vector<SWD_charuco3DMark>& marks,
                                               const cv::Mat& leftImage,
                                               const cv::Mat& rightImage,
                                               int errorCode)
{
    QJsonObject resultObj;
    resultObj["msg_type"] = "mark_result";
    resultObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();
    resultObj["error_code"] = errorCode;
    resultObj["mark_count"] = static_cast<int>(marks.size());

    // 添加标记数据
    QJsonArray marksArray;
    for (const auto& mark : marks)
    {
        QJsonObject markObj;
        markObj["mark_id"] = mark.markID;
        markObj["x"] = mark.mark3D.x;
        markObj["y"] = mark.mark3D.y;
        markObj["z"] = mark.mark3D.z;
        marksArray.append(markObj);
    }
    resultObj["marks"] = marksArray;

#if 0
    // 添加图像（可选，Base64编码）
    if (!leftImage.empty())
    {
        resultObj["left_image"] = imageToBase64(leftImage);
    }
    if (!rightImage.empty())
    {
        resultObj["right_image"] = imageToBase64(rightImage);
    }
#endif
    QJsonDocument doc(resultObj);
    QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
    QByteArray frameData = buildFrame(jsonData);

    // 发送给所有客户端
    if (m_pTcpServer != nullptr)
    {
        m_pTcpServer->SendAllData(frameData.data(), frameData.size());
    }

    LOG_INFO("Sent mark result, mark_count: %zu, error_code: %d\n", marks.size(), errorCode);
}

QString BinocularMarkTcpProtocol::imageToBase64(const cv::Mat& image)
{
    // 将cv::Mat编码为JPEG格式
    std::vector<uchar> buf;
    cv::imencode(".jpg", image, buf);

    // 转换为Base64
    QByteArray ba(reinterpret_cast<const char*>(buf.data()), buf.size());
    return ba.toBase64();
}

QString BinocularMarkTcpProtocol::generateClientId(const TCPClient* pClient)
{
    return QString::number(reinterpret_cast<qintptr>(pClient));
}