GrabBag/Module/BinocularMarkReceiver/Src/BinocularMarkReceiver.cpp

#include "BinocularMarkReceiver.h"
#include <QDebug>

// 静态工厂方法实现
int IBinocularMarkReceiver::CreateInstance(IBinocularMarkReceiver** ppReceiver)
{
    if (!ppReceiver) {
        return -1;
    }

    *ppReceiver = new BinocularMarkReceiver();
    return 0;
}

BinocularMarkReceiver::BinocularMarkReceiver(QObject *parent)
    : QObject(parent)
    , m_pTcpSocket(new QTcpSocket(this))
    , m_pHeartbeatTimer(new QTimer(this))
    , m_nHeartbeatInterval(30)
    , m_serverPort(0)
    , m_bConnected(false)
{
    // 连接TCP套接字信号
    connect(m_pTcpSocket, &QTcpSocket::connected, this, &BinocularMarkReceiver::onConnected);
    connect(m_pTcpSocket, &QTcpSocket::disconnected, this, &BinocularMarkReceiver::onDisconnected);
    connect(m_pTcpSocket, QOverload<QAbstractSocket::SocketError>::of(&QAbstractSocket::error),
            this, &BinocularMarkReceiver::onError);
    connect(m_pTcpSocket, &QTcpSocket::readyRead, this, &BinocularMarkReceiver::onReadyRead);

    // 连接心跳定时器
    connect(m_pHeartbeatTimer, &QTimer::timeout, this, &BinocularMarkReceiver::onHeartbeatTimeout);
}

BinocularMarkReceiver::~BinocularMarkReceiver()
{
    Disconnect();
}

int BinocularMarkReceiver::Connect(const std::string& serverIp, quint16 serverPort)
{
    std::lock_guard<std::mutex> lock(m_mutex);

    if (m_bConnected) {
        qDebug() << "已经连接到服务器";
        return 0;
    }

    m_serverIp = serverIp;
    m_serverPort = serverPort;

    // 连接到服务器
    m_pTcpSocket->connectToHost(QString::fromStdString(serverIp), serverPort);

    // 等待连接（超时5秒）
    if (!m_pTcpSocket->waitForConnected(5000)) {
        qDebug() << "连接BinocularMarkApp服务器失败:" << m_pTcpSocket->errorString();
        return -1;
    }

    return 0;
}

int BinocularMarkReceiver::Disconnect()
{
    std::lock_guard<std::mutex> lock(m_mutex);

    if (!m_bConnected) {
        return 0;
    }

    // 停止心跳
    StopHeartbeat();

    // 断开连接
    m_pTcpSocket->disconnectFromHost();

    // 等待断开连接（超时3秒）
    if (m_pTcpSocket->state() != QAbstractSocket::UnconnectedState) {
        m_pTcpSocket->waitForDisconnected(3000);
    }

    m_bConnected = false;
    m_dataBuffer.clear();

    return 0;
}

bool BinocularMarkReceiver::IsConnected() const
{
    std::lock_guard<std::mutex> lock(m_mutex);
    return m_bConnected;
}

int BinocularMarkReceiver::SendHeartbeat()
{
    QJsonObject jsonObj;
    jsonObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();

    return sendJsonMessage("HEARTBEAT", jsonObj);
}

int BinocularMarkReceiver::TriggerDetection()
{
    QJsonObject jsonObj;
    jsonObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();

    return sendJsonMessage("CMD_TRIGGER", jsonObj);
}

void BinocularMarkReceiver::SetMarkResultCallback(MarkResultCallback callback)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_markResultCallback = callback;
}

void BinocularMarkReceiver::SetConnectionStatusCallback(ConnectionStatusCallback callback)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_connectionCallback = callback;
}

void BinocularMarkReceiver::StartHeartbeat(int intervalSeconds)
{
    m_nHeartbeatInterval = intervalSeconds;
    m_pHeartbeatTimer->start(intervalSeconds * 1000);
}

void BinocularMarkReceiver::StopHeartbeat()
{
    m_pHeartbeatTimer->stop();
}

void BinocularMarkReceiver::onConnected()
{
    qDebug() << "已连接到BinocularMarkApp服务器";

    {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_bConnected = true;
    }

    // 通知连接状态改变
    if (m_connectionCallback) {
        m_connectionCallback(true);
    }

    // 启动心跳
    StartHeartbeat(m_nHeartbeatInterval);
}

void BinocularMarkReceiver::onDisconnected()
{
    qDebug() << "与BinocularMarkApp服务器断开连接";

    {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_bConnected = false;
    }

    // 停止心跳
    StopHeartbeat();

    // 通知连接状态改变
    if (m_connectionCallback) {
        m_connectionCallback(false);
    }
}

void BinocularMarkReceiver::onError(QAbstractSocket::SocketError socketError)
{
    qDebug() << "TCP套接字错误:" << socketError << m_pTcpSocket->errorString();
}

void BinocularMarkReceiver::onReadyRead()
{
    // 读取所有可用数据
    QByteArray data = m_pTcpSocket->readAll();

    // 追加到缓冲区
    m_dataBuffer.append(data);

    // 解析数据帧
    parseFrames(m_dataBuffer);
}

void BinocularMarkReceiver::onHeartbeatTimeout()
{
    // 发送心跳消息
    SendHeartbeat();
}

void BinocularMarkReceiver::parseFrames(const QByteArray& data)
{
    while (true) {
        // 查找帧头
        int headerIndex = m_dataBuffer.indexOf(FRAME_HEADER);
        if (headerIndex == -1) {
            // 没有找到帧头，清空缓冲区
            m_dataBuffer.clear();
            break;
        }

        // 丢弃帧头之前的数据
        if (headerIndex > 0) {
            m_dataBuffer.remove(0, headerIndex);
        }

        // 检查是否有足够的数据来读取长度字段
        if (m_dataBuffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE) {
            // 数据不够，等待更多数据
            break;
        }

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

        if (!ok) {
            qDebug() << "无效的长度字符串:" << lengthStr;
            m_dataBuffer.remove(0, FRAME_HEADER_SIZE);
            continue;
        }

        // 检查数据长度是否合理（最大10MB）
        if (dataLength < 0 || dataLength > 10 * 1024 * 1024) {
            qDebug() << "无效的数据长度:" << dataLength;
            m_dataBuffer.remove(0, FRAME_HEADER_SIZE);
            continue;
        }

        // 计算完整帧的总长度
        int totalFrameLength = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE;

        // 检查是否有完整的帧
        if (m_dataBuffer.size() < totalFrameLength) {
            // 数据不够，等待更多数据
            break;
        }

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

        // 验证帧尾
        QByteArray tail = m_dataBuffer.mid(FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength, FRAME_TAIL_SIZE);
        if (tail != FRAME_TAIL) {
            qDebug() << "无效的帧尾";
            m_dataBuffer.remove(0, FRAME_HEADER_SIZE);
            continue;
        }

        // 处理JSON消息
        handleJsonMessage(jsonData);

        // 移除已处理的帧
        m_dataBuffer.remove(0, totalFrameLength);
    }
}

void BinocularMarkReceiver::handleJsonMessage(const QByteArray& jsonData)
{
    // 解析JSON
    QJsonParseError parseError;
    QJsonDocument jsonDoc = QJsonDocument::fromJson(jsonData, &parseError);

    if (parseError.error != QJsonParseError::NoError) {
        qDebug() << "JSON解析错误:" << parseError.errorString();
        return;
    }

    if (!jsonDoc.isObject()) {
        qDebug() << "JSON不是对象";
        return;
    }

    QJsonObject jsonObj = jsonDoc.object();

    // 获取消息类型
    QString messageType = jsonObj["message_type"].toString();

    if (messageType == "MARK_RESULT") {
        handleMarkResult(jsonObj);
    } else if (messageType == "HEARTBEAT_ACK") {
        handleHeartbeatAck(jsonObj);
    } else if (messageType == "CMD_RESPONSE") {
        handleCommandResponse(jsonObj);
    } else {
        qDebug() << "未知的消息类型:" << messageType;
    }
}

void BinocularMarkReceiver::handleMarkResult(const QJsonObject& jsonObj)
{
    // 解析Mark结果
    qint64 timestamp = jsonObj["timestamp"].toVariant().toLongLong();
    int errorCode = jsonObj["error_code"].toInt();

    qDebug() << "========== BinocularMark 接收数据 ==========";
    qDebug() << "时间戳 (timestamp):" << timestamp;
    qDebug() << "错误码 (error_code):" << errorCode;

    std::vector<VrMark3D> marks;

    if (errorCode == 0) {
        QJsonArray marksArray = jsonObj["marks"].toArray();
        qDebug() << "Mark 点数量:" << marksArray.size();

        int index = 0;
        for (const QJsonValue& markValue : marksArray) {
            QJsonObject markObj = markValue.toObject();

            VrMark3D mark;
            mark.markID = markObj["mark_id"].toInt();
            mark.x = markObj["x"].toDouble();
            mark.y = markObj["y"].toDouble();
            mark.z = markObj["z"].toDouble();

            qDebug() << QString("  Mark[%1]: ID=%2, X=%3, Y=%4, Z=%5")
                        .arg(index)
                        .arg(mark.markID)
                        .arg(mark.x, 0, 'f', 3)
                        .arg(mark.y, 0, 'f', 3)
                        .arg(mark.z, 0, 'f', 3);

            marks.push_back(mark);
            index++;
        }
    } else {
        qDebug() << "检测失败，错误码:" << errorCode;
    }
    qDebug() << "==========================================";

    // 调用回调函数
    if (m_markResultCallback) {
        m_markResultCallback(marks, timestamp, errorCode);
    }
}

void BinocularMarkReceiver::handleHeartbeatAck(const QJsonObject& jsonObj)
{
    // 心跳应答，暂时不处理
    qDebug() << "收到心跳应答";
}

void BinocularMarkReceiver::handleCommandResponse(const QJsonObject& jsonObj)
{
    QString cmdType = jsonObj["cmd_type"].toString();
    bool result = jsonObj["result"].toBool();
    int errorCode = jsonObj["error_code"].toInt();
    QString errorMsg = jsonObj["error_msg"].toString();

    qDebug() << "命令应答:" << cmdType << "结果:" << result << "错误码:" << errorCode << "错误消息:" << errorMsg;
}

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

    // 添加帧头
    frame.append(FRAME_HEADER, FRAME_HEADER_SIZE);

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

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

    // 添加帧尾
    frame.append(FRAME_TAIL, FRAME_TAIL_SIZE);

    return frame;
}

int BinocularMarkReceiver::sendJsonMessage(const QString& messageType, const QJsonObject& jsonObj)
{
    if (!m_bConnected) {
        qDebug() << "未连接到服务器";
        return -1;
    }

    // 构造JSON对象
    QJsonObject msgObj = jsonObj;
    msgObj["message_type"] = messageType;

    // 转换为JSON字符串
    QJsonDocument jsonDoc(msgObj);
    QByteArray jsonData = jsonDoc.toJson(QJsonDocument::Compact);

    // 构造数据帧
    QByteArray frame = buildFrame(jsonData);

    // 发送数据
    qint64 bytesWritten = m_pTcpSocket->write(frame);
    if (bytesWritten == -1) {
        qDebug() << "发送数据失败:" << m_pTcpSocket->errorString();
        return -1;
    }

    m_pTcpSocket->flush();
    return 0;
}