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;
    }

    const std::string sendData = text + "\r\n";
    bool success = false;
    if (pClient) {
        success = m_pTCPServer->SendData(pClient, sendData.c_str(), static_cast<unsigned int>(sendData.size()));
    } else {
        success = m_pTCPServer->SendAllData(sendData.c_str(), static_cast<unsigned int>(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("\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()) {
            ParseTextCommand(pClient, line);
        }
    }
}

void ScrewPositionTCPProtocol::ParseTextCommand(const TCPClient* pClient, const QByteArray& line)
{
    Q_UNUSED(pClient);

    // Protocol format: T1_X_Y_Z_A_B_C
    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;
    }

    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;
    }

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

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