GrabBag/App/DiscHolePose/DiscHolePoseApp/Presenter/Src/DiscHolePoseTCPProtocol.cpp

#include "DiscHolePoseTCPProtocol.h"

#include "IYTCPServer.h"
#include "IVrConfig.h"
#include "VrLog.h"

#include <QString>
#include <QStringList>
#include <QMetaObject>
#include <map>

// ============================================================
// Impl —— 持有 IYTCPServer 生命周期与行缓冲，通过回调驱动 QObject 信号
// ============================================================
class DiscHolePoseTCPProtocol::Impl
{
public:
    DiscHolePoseTCPProtocol* q = nullptr;

    IYTCPServer* m_pTCPServer = nullptr;
    bool m_bServerRunning = false;
    uint16_t m_nPort = 0;
    bool m_bConnected = false;

    DetectionTriggerCallback m_detectionTriggerCallback;

    // 每个客户端的行缓冲（基于 TCPClient 指针作为 key）
    std::map<const TCPClient*, QByteArray> m_clientBuffers;

    // ============ 生命周期 ============

    bool StartServer(uint16_t port)
    {
        m_nPort = port;

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

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

        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("DiscHolePose: Failed to start TCP server\n");
            m_pTCPServer->Close();
            delete m_pTCPServer;
            m_pTCPServer = nullptr;
            return false;
        }

        m_bServerRunning = true;
        LOG_DEBUG("DiscHolePose TCP protocol started on port %d\n", port);
        return true;
    }

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

    bool IsConnected() const
    {
        return m_bConnected;
    }

    // ============ 发送 ============

    int SendResult(const QString& resultText, const TCPClient* pClient = nullptr)
    {
        if (!m_pTCPServer || !m_bServerRunning) {
            return -1;
        }

        const QByteArray sendData = resultText.toUtf8() + "\r\n";
        bool success = false;
        if (pClient) {
            success = m_pTCPServer->SendData(pClient, sendData.constData(),
                                             static_cast<unsigned int>(sendData.size()));
        } else {
            success = m_pTCPServer->SendAllData(sendData.constData(),
                                                static_cast<unsigned int>(sendData.size()));
        }

        return success ? 0 : -2;
    }

    // ============ TCP 事件回调（在 IYTCPServer 工作线程中）============

    void OnTCPEvent(const TCPClient* pClient, TCPServerEventType eventType)
    {
        switch (eventType) {
        case TCP_EVENT_CLIENT_CONNECTED:
            LOG_DEBUG("DiscHolePose: TCP client connected: %p\n", pClient);
            m_bConnected = true;
            QMetaObject::invokeMethod(q, [this]() {
                emit q->ConnectionChanged(true);
            }, Qt::QueuedConnection);
            break;
        case TCP_EVENT_CLIENT_DISCONNECTED:
            LOG_DEBUG("DiscHolePose: TCP client disconnected: %p\n", pClient);
            m_clientBuffers.erase(pClient);
            m_bConnected = false;
            QMetaObject::invokeMethod(q, [this]() {
                emit q->ConnectionChanged(false);
            }, Qt::QueuedConnection);
            break;
        case TCP_EVENT_CLIENT_EXCEPTION:
            LOG_WARNING("DiscHolePose: TCP client exception: %p\n", pClient);
            m_clientBuffers.erase(pClient);
            m_bConnected = false;
            QMetaObject::invokeMethod(q, [this]() {
                emit q->ConnectionChanged(false);
            }, Qt::QueuedConnection);
            break;
        }
    }

    void OnTCPDataReceived(const TCPClient* pClient, const char* pData, unsigned int nLen)
    {
        if (!pData || nLen == 0) {
            return;
        }

        m_clientBuffers[pClient].append(pData, static_cast<int>(nLen));
        QByteArray& buffer = m_clientBuffers[pClient];

        while (true) {
            int idx = buffer.indexOf("\r\n");
            int terminatorLen = 2;
            if (idx < 0) {
                idx = buffer.indexOf('\n');
                terminatorLen = 1;
            }
            if (idx < 0) {
                break;
            }

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

            if (!line.isEmpty()) {
                const QString strLine = QString::fromUtf8(line);

                // Emit DataReceived signal on main thread
                QMetaObject::invokeMethod(q, [this, strLine]() {
                    emit q->DataReceived(strLine);
                }, Qt::QueuedConnection);

                ProcessCommand(strLine);
            }
        }
    }

    // ============ 命令解析 ============

    void ProcessCommand(const QString& command)
    {
        // 协议格式: T1_X_Y_Z_A_B_C
        //   T = D（discHole）或 R（discRack）
        //   1 = 相机索引（从 1 开始）
        QStringList tokens = command.split('_', Qt::SkipEmptyParts);

        LOG_DEBUG("DiscHolePose ParseCommand: '%s', tokens=%d\n",
                  command.toStdString().c_str(), tokens.size());

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

        // 解析检测类型
        const QString typeToken = tokens[0].toUpper();
        DetectionType detectionType;
        if (typeToken.startsWith('D')) {
            detectionType = DETECTION_TYPE_DISC_HOLE;
        } else if (typeToken.startsWith('R')) {
            detectionType = DETECTION_TYPE_DISC_RACK;
        } else {
            LOG_ERROR("DiscHolePose: Invalid type token: %s (expected D or R)\n",
                      typeToken.toStdString().c_str());
            return;
        }

        // 解析相机索引
        int cameraIndex = typeToken.mid(1).toInt();
        if (cameraIndex < 1) {
            cameraIndex = 1;
        }

        // 解析机器人位姿 X_Y_Z_A_B_C
        RobotPose6D robotPose;
        double* values[] = {
            &robotPose.x, &robotPose.y, &robotPose.z,
            &robotPose.a, &robotPose.b, &robotPose.c
        };

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

        LOG_INFO("DiscHolePose 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.a, robotPose.b, robotPose.c);

        // Emit DetectionTriggered signal on main thread
        QMetaObject::invokeMethod(q, [this, cameraIndex, detectionType, robotPose]() {
            emit q->DetectionTriggered(cameraIndex, detectionType, robotPose);
        }, Qt::QueuedConnection);

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

// ============================================================
// DiscHolePoseTCPProtocol 公开接口（委托给 Impl）
// ============================================================

DiscHolePoseTCPProtocol::DiscHolePoseTCPProtocol(QObject* parent)
    : QObject(parent)
    , m_pImpl(std::make_unique<Impl>())
{
    m_pImpl->q = this;
}

DiscHolePoseTCPProtocol::~DiscHolePoseTCPProtocol()
{
    StopServer();
}

bool DiscHolePoseTCPProtocol::StartServer(uint16_t port)
{
    return m_pImpl->StartServer(port);
}

void DiscHolePoseTCPProtocol::StopServer()
{
    m_pImpl->StopServer();
}

bool DiscHolePoseTCPProtocol::IsConnected() const
{
    return m_pImpl->IsConnected();
}

void DiscHolePoseTCPProtocol::SetDetectionTriggerCallback(DetectionTriggerCallback callback)
{
    m_pImpl->m_detectionTriggerCallback = std::move(callback);
}

void DiscHolePoseTCPProtocol::SendResult(const QString& resultText)
{
    m_pImpl->SendResult(resultText);
}

// Private slots —— 这些槽在当前设计中由 Impl 的回调通过 QMetaObject::invokeMethod
// 驱动信号，因此这里保留空实现（实际逻辑在 Impl::OnTCPEvent / OnTCPDataReceived 内完成）。
// 如果将来需要外部主动调用这些槽，可以补充转发逻辑。
void DiscHolePoseTCPProtocol::OnNewConnection()
{
    // 当前由 Impl 内部 TCP_EVENT_CLIENT_CONNECTED 直接 emit ConnectionChanged
}

void DiscHolePoseTCPProtocol::OnDisconnected()
{
    // 当前由 Impl 内部 TCP_EVENT_CLIENT_DISCONNECTED 直接 emit ConnectionChanged
}

void DiscHolePoseTCPProtocol::OnReadyRead(const QByteArray& data)
{
    // 当前数据接收在 Impl::OnTCPDataReceived 中完成，
    // 此槽保留供将来 Qt 信号驱动的接收路径使用
    Q_UNUSED(data);
}

void DiscHolePoseTCPProtocol::ProcessCommand(const QString& command)
{
    m_pImpl->ProcessCommand(command);
}