GrabBag/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionTCPProtocol.cpp

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

ScrewPositionTCPProtocol::ScrewPositionTCPProtocol()
{
}

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

int ScrewPositionTCPProtocol::Initialize(uint16_t 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;
    LOG_DEBUG("TCP text protocol initialized on port %d\n", port);
    return 0;
}

void ScrewPositionTCPProtocol::Deinitialize()
{
    if (m_pTCPServer) {
        m_bServerRunning = false;
        m_pTCPServer->Stop();
        m_pTCPServer->Close();
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
        m_clientBuffers.clear();
    }
}

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

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

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

int ScrewPositionTCPProtocol::SendTextResult(const std::string& text, const TCPClient* pClient)
{
    if (!m_pTCPServer || !m_bServerRunning) {
        return -1;
    }

    std::string sendData = text + "\n";
    bool success = false;
    if (pClient) {
        success = m_pTCPServer->SendData(pClient, sendData.c_str(), sendData.size());
    } else {
        success = m_pTCPServer->SendAllData(sendData.c_str(), sendData.size());
    }

    return success ? 0 : -2;
}

void ScrewPositionTCPProtocol::OnTCPEvent(const TCPClient* pClient, TCPServerEventType eventType)
{
    switch (eventType) {
        case TCP_EVENT_CLIENT_CONNECTED:
            LOG_DEBUG("TCP client connected: %p\n", pClient);
            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 ScrewPositionTCPProtocol::OnTCPDataReceived(const TCPClient* pClient, const char* pData, unsigned int nLen)
{
    if (!pData || nLen == 0) return;

    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 ScrewPositionTCPProtocol::ParseTextCommand(const TCPClient* pClient, const QByteArray& line)
{
    // 协议格式：S1_-23.45_200.30_50.10_15.50_-2.30_45.00
    // S=螺杆检测, T=工具盘检测, 后跟相机索引, _分隔 X Y Z RX RY RZ
    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;
    }

    // 解析类型和相机索引：S1/T1 或 S2/T2
    QString typeToken = tokens[0].toUpper();
    DetectionType detectionType = DETECTION_TYPE_SCREW;
    if (typeToken.startsWith('S')) {
        detectionType = DETECTION_TYPE_SCREW;
    } else if (typeToken.startsWith('T')) {
        detectionType = DETECTION_TYPE_TOOL_DISK;
    } else {
        LOG_ERROR("Invalid type token: %s (expected S or T)\n", typeToken.toStdString().c_str());
        return;
    }

    int cameraIndex = typeToken.mid(1).toInt();
    if (cameraIndex < 1) {
        cameraIndex = 1;
    }

    // 解析机器人位姿：X Y Z RX RY RZ
    RobotPose6D robotPose;
    double* values[] = {&robotPose.x, &robotPose.y, &robotPose.z,
                        &robotPose.rx, &robotPose.ry, &robotPose.rz};

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

    LOG_INFO("TCP trigger: type=%d, camera=%d, pose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n",
             static_cast<int>(detectionType), cameraIndex,
             robotPose.x, robotPose.y, robotPose.z,
             robotPose.rx, robotPose.ry, robotPose.rz);

    if (m_detectionTriggerCallback) {
        m_detectionTriggerCallback(cameraIndex, detectionType, robotPose);
    }
}