GrabBag/App/HolePitPosition/HolePitPositionApp/Presenter/Src/TCPServerProtocol.cpp

#include "TCPServerProtocol.h"
#include "VrLog.h"
#include <QString>
#include <QStringList>

TCPServerProtocol::TCPServerProtocol()
    : m_pTCPServer(nullptr)
    , m_bServerRunning(false)
    , m_nPort(5020)
    , m_connectionStatus(CONNECTION_DISCONNECTED)
{
}

TCPServerProtocol::~TCPServerProtocol()
{
    Deinitialize();
}

int TCPServerProtocol::Initialize(uint16_t port)
{
    LOG_DEBUG("Initializing TCP server protocol on port %d\n", port);

    m_nPort = port;

    if (!VrCreatYTCPServer(&m_pTCPServer)) {
        LOG_ERROR("Failed to create TCP server instance\n");
        return -1;
    }

    if (!m_pTCPServer->Init(port)) {
        LOG_ERROR("Failed to initialize TCP server on port %d\n", port);
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
        return -2;
    }

    m_pTCPServer->SetEventCallback([this](const TCPClient* pClient, TCPServerEventType eventType) {
        this->OnTCPEvent(pClient, eventType);
    });

    if (!m_pTCPServer->Start([this](const TCPClient* pClient, const char* pData, const unsigned int nLen) {
        this->OnTCPDataReceived(pClient, pData, nLen);
    })) {
        LOG_ERROR("Failed to start TCP server\n");
        m_pTCPServer->Close();
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
        return -3;
    }

    m_bServerRunning = true;
    m_connectionStatus = CONNECTION_CONNECTED;

    LOG_DEBUG("TCP server protocol initialized successfully on port %d\n", port);
    return 0;
}

void TCPServerProtocol::Deinitialize()
{
    if (m_pTCPServer) {
        LOG_DEBUG("Stopping TCP server protocol\n");

        m_bServerRunning = false;
        m_connectionStatus = CONNECTION_DISCONNECTED;

        m_pTCPServer->Stop();
        m_pTCPServer->Close();
        delete m_pTCPServer;
        m_pTCPServer = nullptr;

        m_clientBuffers.clear();

        LOG_DEBUG("TCP server protocol stopped\n");
    }
}

int TCPServerProtocol::SendTextResult(const std::string& text, const TCPClient* pClient)
{
    if (!m_pTCPServer || !m_bServerRunning) {
        LOG_ERROR("TCP server is not running\n");
        return -1;
    }

    // 添加换行符作为消息分隔
    std::string sendData = text + "\n";

    bool success = false;
    if (pClient) {
        success = m_pTCPServer->SendData(pClient, sendData.c_str(), sendData.size());
        LOG_DEBUG("Sent text result to specific client, size: %zu bytes\n", sendData.size());
    } else {
        success = m_pTCPServer->SendAllData(sendData.c_str(), sendData.size());
        LOG_DEBUG("Sent text result to all clients, size: %zu bytes\n", sendData.size());
    }

    if (!success) {
        LOG_ERROR("Failed to send text result\n");
        return -2;
    }

    return 0;
}

TCPServerProtocol::TCPStatus TCPServerProtocol::GetConnectionStatus() const
{
    return m_connectionStatus;
}

void TCPServerProtocol::SetConnectionCallback(const ConnectionCallback& callback)
{
    m_connectionCallback = callback;
}

void TCPServerProtocol::SetDetectionTriggerCallback(const DetectionTriggerCallback& callback)
{
    m_detectionTriggerCallback = callback;
}

bool TCPServerProtocol::IsRunning() const
{
    return m_bServerRunning;
}

void TCPServerProtocol::OnTCPEvent(const TCPClient* pClient, TCPServerEventType eventType)
{
    switch (eventType) {
        case TCP_EVENT_CLIENT_CONNECTED:
            LOG_DEBUG("TCP client connected: %p\n", pClient);
            m_connectionStatus = CONNECTION_CONNECTED;
            if (m_connectionCallback) {
                m_connectionCallback(true);
            }
            break;

        case TCP_EVENT_CLIENT_DISCONNECTED:
            LOG_DEBUG("TCP client disconnected: %p\n", pClient);
            // 清理该客户端的缓冲区
            m_clientBuffers.erase(pClient);
            if (m_connectionCallback) {
                m_connectionCallback(false);
            }
            break;

        case TCP_EVENT_CLIENT_EXCEPTION:
            LOG_WARNING("TCP client exception: %p\n", pClient);
            m_clientBuffers.erase(pClient);
            if (m_connectionCallback) {
                m_connectionCallback(false);
            }
            break;
    }
}

void TCPServerProtocol::OnTCPDataReceived(const TCPClient* pClient, const char* pData, unsigned int nLen)
{
    if (!pData || nLen == 0) {
        LOG_WARNING("Received empty data from client %p\n", pClient);
        return;
    }

    LOG_DEBUG("Received TCP data from client %p, size: %u bytes\n", pClient, nLen);

    // 追加数据到该客户端的缓冲区
    m_clientBuffers[pClient].append(pData, nLen);

    // 按换行符分割完整行
    QByteArray& buffer = m_clientBuffers[pClient];
    while (true) {
        int idx = buffer.indexOf('\n');
        if (idx < 0) break;

        QByteArray line = buffer.left(idx).trimmed();
        buffer.remove(0, idx + 1);

        if (!line.isEmpty()) {
            ParseTextCommand(pClient, line);
        }
    }
}

void TCPServerProtocol::ParseTextCommand(const TCPClient* pClient, const QByteArray& line)
{
    // 协议格式：X1 RbtX-23.45 RbtY-23.45 RbtZ-23.45 RbtRoll-23.45 RbtPitch-123.45 RbtYaw-23.45
    QString strLine = QString::fromUtf8(line);
    QStringList tokens = strLine.split(' ', QString::SkipEmptyParts);

    LOG_DEBUG("ParseTextCommand: line='%s', tokens=%d\n", strLine.toStdString().c_str(), tokens.size());

    if (tokens.size() < 7) {
        LOG_ERROR("Invalid command format, expected 7 tokens, got %d: %s\n",
                  tokens.size(), strLine.toStdString().c_str());
        return;
    }

    // 解析相机索引：X1 或 X2
    QString cameraToken = tokens[0];
    if (!cameraToken.startsWith('X') && !cameraToken.startsWith('x')) {
        LOG_ERROR("Invalid camera token: %s\n", cameraToken.toStdString().c_str());
        return;
    }
    int cameraIndex = cameraToken.mid(1).toInt();
    if (cameraIndex < 1 || cameraIndex > 2) {
        LOG_ERROR("Invalid camera index: %d\n", cameraIndex);
        return;
    }

    // 解析机器人法兰位姿
    // 每个 token 格式为 "RbtX-23.45"，前缀为 RbtX/RbtY/RbtZ/RbtRoll/RbtPitch/RbtYaw
    RobotFlangePose robotPose;
    const QString prefixes[] = {"RbtX", "RbtY", "RbtZ", "RbtRoll", "RbtPitch", "RbtYaw"};
    double* values[] = {&robotPose.x, &robotPose.y, &robotPose.z,
                        &robotPose.roll, &robotPose.pitch, &robotPose.yaw};

    for (int i = 0; i < 6; i++) {
        QString token = tokens[i + 1];
        if (!token.startsWith(prefixes[i])) {
            LOG_ERROR("Invalid token prefix at position %d: expected '%s', got '%s'\n",
                      i + 1, prefixes[i].toStdString().c_str(), token.toStdString().c_str());
            return;
        }
        QString valueStr = token.mid(prefixes[i].length());
        bool ok = false;
        double val = valueStr.toDouble(&ok);
        if (!ok) {
            LOG_ERROR("Invalid numeric value at position %d: '%s'\n",
                      i + 1, valueStr.toStdString().c_str());
            return;
        }
        *values[i] = val;
    }

    LOG_INFO("Parsed trigger: camera=%d, pose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n",
             cameraIndex, robotPose.x, robotPose.y, robotPose.z,
             robotPose.roll, robotPose.pitch, robotPose.yaw);

    // 触发检测回调
    if (m_detectionTriggerCallback) {
        m_detectionTriggerCallback(cameraIndex, robotPose);
    } else {
        LOG_WARNING("Detection trigger callback not set\n");
    }
}