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

/**
 * @file TCPServerMethods.cpp
 * @brief ScrewPositionPresenter TCP/Modbus 通信相关方法实现
 */

#include "ScrewPositionPresenter.h"
#include "DetectionOutputConverter.h"
#include "VrLog.h"
#include <array>
#include <cstring>
#include <cstdio>

namespace {

constexpr uint16_t kModbusTriggerAddress = 0;
constexpr uint16_t kModbusWorkStatusAddress = 1;
constexpr uint16_t kModbusRobotPoseAddress = 2;       // 机械臂位姿写入起始地址
constexpr uint16_t kModbusRobotPoseRegisterCount = 12; // 6个float32 = 12个寄存器
constexpr uint16_t kModbusResultStartAddress = 14;     // 检测结果起始地址（2+12=14）
constexpr uint16_t kModbusResultRegisterCount = 12;

void FloatToRegisters(float value, uint16_t& high, uint16_t& low)
{
    uint32_t raw = 0;
    static_assert(sizeof(raw) == sizeof(value), "float size mismatch");
    std::memcpy(&raw, &value, sizeof(value));
    high = static_cast<uint16_t>((raw >> 16) & 0xFFFF);
    low = static_cast<uint16_t>(raw & 0xFFFF);
}

float RegistersToFloat(uint16_t high, uint16_t low)
{
    uint32_t raw = (static_cast<uint32_t>(high) << 16) | static_cast<uint32_t>(low);
    float value = 0;
    std::memcpy(&value, &raw, sizeof(value));
    return value;
}

template<typename PoseT>
void FillPoseRegisters(const PoseT& pose, std::array<uint16_t, kModbusResultRegisterCount>& registers)
{
    const float values[6] = {
        static_cast<float>(pose.x),
        static_cast<float>(pose.y),
        static_cast<float>(pose.z),
        static_cast<float>(pose.roll),
        static_cast<float>(pose.pitch),
        static_cast<float>(pose.yaw)
    };

    for (size_t i = 0; i < 6; ++i) {
        FloatToRegisters(values[i], registers[i * 2], registers[i * 2 + 1]);
    }
}

}

void ScrewPositionPresenter::_SendDetectionResultToTCP(const DetectionResult& result, int cameraIndex)
{
    if (!m_pTCPServer || !m_pTCPServer->IsRunning()) {
        LOG_WARNING("TCP protocol not running, skip sending result\n");
        return;
    }

    // 文本格式：PointNum_X1_Y1_Z1_RX1_RY1_RZ1/X2_Y2_Z2_RX2_RY2_RZ2/
    int count = static_cast<int>(result.positions.size());
    std::string resultText = std::to_string(count);

    for (size_t i = 0; i < result.positions.size(); ++i) {
        const auto& pos = result.positions[i];
        char buf[256];
        snprintf(buf, sizeof(buf), "_%.2f_%.2f_%.2f_%.2f_%.2f_%.2f/",
                 pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
        resultText += buf;
    }

    LOG_INFO("TCP result text: %s\n", resultText.c_str());

    int ret = m_pTCPServer->SendTextResult(resultText);
    if (ret != 0) {
        LOG_ERROR("Failed to send detection result via TCP, error: %d\n", ret);
    }
}

void ScrewPositionPresenter::_PublishDetectionResultToModbus(const DetectionResult& result)
{
    if (!IsModbusServerRunning()) {
        return;
    }

    std::array<uint16_t, kModbusResultRegisterCount> registers{};
    bool hasPose = false;

    if (!result.positions.empty()) {
        const auto& pos = result.positions.front();
        const float values[6] = {
            static_cast<float>(pos.x),
            static_cast<float>(pos.y),
            static_cast<float>(pos.z),
            static_cast<float>(pos.roll),
            static_cast<float>(pos.pitch),
            static_cast<float>(pos.yaw)
        };
        for (size_t i = 0; i < 6; ++i) {
            FloatToRegisters(values[i], registers[i * 2], registers[i * 2 + 1]);
        }
        hasPose = true;
    }

    WriteModbusRegisters(kModbusResultStartAddress, registers.data(), kModbusResultRegisterCount);
    LOG_DEBUG("Published Modbus pose registers, hasPose=%d\n", hasPose ? 1 : 0);

    _UpdateModbusWorkStatus(result.success ? 2 : 3);
}

void ScrewPositionPresenter::_ResetModbusResultRegisters()
{
    if (!IsModbusServerRunning()) {
        return;
    }

    std::array<uint16_t, kModbusResultRegisterCount> zeros{};
    WriteModbusRegisters(kModbusResultStartAddress, zeros.data(), kModbusResultRegisterCount);
}

void ScrewPositionPresenter::_UpdateModbusWorkStatus(uint16_t statusValue)
{
    if (!IsModbusServerRunning()) {
        m_modbusWorkStatus = statusValue;
        return;
    }

    m_modbusWorkStatus = statusValue;
    WriteModbusRegisters(kModbusWorkStatusAddress, &statusValue, 1);
}

void ScrewPositionPresenter::_InitializeModbusRegisters()
{
    if (m_modbusRegistersInitialized || !IsModbusServerRunning()) {
        return;
    }

    const uint16_t zero = 0;
    WriteModbusRegisters(kModbusTriggerAddress, &zero, 1);

    // 清零机械臂位姿寄存器
    std::array<uint16_t, kModbusRobotPoseRegisterCount> robotZeros{};
    WriteModbusRegisters(kModbusRobotPoseAddress, robotZeros.data(), kModbusRobotPoseRegisterCount);

    _ResetModbusResultRegisters();
    _UpdateModbusWorkStatus(0);
    m_modbusRegistersInitialized = true;
}

RobotPose6D ScrewPositionPresenter::_ReadRobotPoseFromModbus()
{
    RobotPose6D pose;
    // 从缓存中的寄存器值读取，OnModbusWriteCallback 中已缓存
    return pose;
}

int ScrewPositionPresenter::InitTCPServer()
{
    if (m_pTCPServer) {
        LOG_WARNING("TCP server already initialized\n");
        return 0;
    }

    m_pTCPServer = new ScrewPositionTCPProtocol();

    m_pTCPServer->SetConnectionCallback([this](bool connected) {
        OnTCPConnectionChanged(connected);
    });

    m_pTCPServer->SetDetectionTriggerCallback(
        [this](int cameraIndex, DetectionType detectionType, const RobotPose6D& robotPose) -> bool {
            LOG_DEBUG("TCP triggered detection, cameraIndex: %d, type: %d\n",
                      cameraIndex, static_cast<int>(detectionType));
            return TriggerDetection(cameraIndex, detectionType, robotPose);
        });

    uint16_t port = 7800;
    if (m_pConfigManager) {
        const ConfigResult configResult = m_pConfigManager->GetConfigResult();
        port = configResult.tcpPort;
    }

    int ret = m_pTCPServer->Initialize(port);
    if (ret != 0) {
        LOG_ERROR("Failed to initialize TCP protocol on port %d, error: %d\n", port, ret);
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
        return ret;
    }

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

void ScrewPositionPresenter::stopServer()
{
    if (m_pTCPServer) {
        m_pTCPServer->Deinitialize();
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
        m_bTCPConnected = false;
        LOG_DEBUG("TCP server stopped\n");
    }
}

void ScrewPositionPresenter::OnTCPConnectionChanged(bool connected)
{
    m_bTCPConnected = connected;
    LOG_DEBUG("TCP connection changed: %s\n", connected ? "connected" : "disconnected");

    if (auto pStatus = GetStatusCallback<IYScrewPositionStatus>()) {
        pStatus->OnRobotConnectionChanged(connected);
        pStatus->OnStatusUpdate(connected ? "TCP客户端已连接" : "TCP客户端已断开");
    }

    CheckAndUpdateWorkStatus();
}

void ScrewPositionPresenter::SendDetectionResultToClient(const DetectionResult& result)
{
    _SendDetectionResultToTCP(result, result.cameraIndex);
}