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

/**
 * @file TCPServerMethods.cpp
 * @brief ScrewPositionPresenter TCP/Modbus communication methods
 */

#include "ScrewPositionPresenter.h"

#include "DetectionOutputConverter.h"
#include "VrLog.h"

#include <array>
#include <cstdio>
#include <cstring>

namespace {

constexpr uint16_t kModbusTriggerAddress = 0;
constexpr uint16_t kModbusWorkStatusAddress = 1;
constexpr uint16_t kModbusRobotPoseAddress = 2;
constexpr uint16_t kModbusRobotPoseRegisterCount = 12;
constexpr uint16_t kModbusResultStartAddress = 14;
// 结果寄存器：24 = 12 floats * 2 regs，先接近点(6 floats)后目标点(6 floats)
constexpr uint16_t kModbusResultRegisterCount = 24;

constexpr int kPoseOutputRxRyRz = 0;
constexpr int kPoseOutputRxRzRy = 1;
constexpr int kPoseOutputRyRxRz = 2;
constexpr int kPoseOutputRyRzRx = 3;
constexpr int kPoseOutputRzRxRy = 4;
constexpr int kPoseOutputRzRyRx = 5;

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

void ReorderPoseAngles(double roll,
                       double pitch,
                       double yaw,
                       int poseOutputOrder,
                       double& angle1,
                       double& angle2,
                       double& angle3)
{
    switch (poseOutputOrder) {
    case kPoseOutputRxRzRy:
        angle1 = roll;
        angle2 = yaw;
        angle3 = pitch;
        break;
    case kPoseOutputRyRxRz:
        angle1 = pitch;
        angle2 = roll;
        angle3 = yaw;
        break;
    case kPoseOutputRyRzRx:
        angle1 = pitch;
        angle2 = yaw;
        angle3 = roll;
        break;
    case kPoseOutputRzRxRy:
        angle1 = yaw;
        angle2 = roll;
        angle3 = pitch;
        break;
    case kPoseOutputRzRyRx:
        angle1 = yaw;
        angle2 = pitch;
        angle3 = roll;
        break;
    case kPoseOutputRxRyRz:
    default:
        angle1 = roll;
        angle2 = pitch;
        angle3 = yaw;
        break;
    }
}

template<typename PoseT>
void FillPoseRegisters(const PoseT& pose,
                       int poseOutputOrder,
                       std::array<uint16_t, kModbusResultRegisterCount>& registers)
{
    double angle1 = 0.0;
    double angle2 = 0.0;
    double angle3 = 0.0;
    ReorderPoseAngles(pose.roll, pose.pitch, pose.yaw, poseOutputOrder, angle1, angle2, angle3);

    // 先接近点（6 floats），再目标点（6 floats）。两者姿态相同，机械臂到接近点后
    // 保持姿态沿工具 Z 轴直线下压到目标点，避免姿态变化碰撞。
    const float values[12] = {
        static_cast<float>(pose.approachX),
        static_cast<float>(pose.approachY),
        static_cast<float>(pose.approachZ),
        static_cast<float>(angle1),
        static_cast<float>(angle2),
        static_cast<float>(angle3),
        static_cast<float>(pose.x),
        static_cast<float>(pose.y),
        static_cast<float>(pose.z),
        static_cast<float>(angle1),
        static_cast<float>(angle2),
        static_cast<float>(angle3)
    };

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

} // namespace

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

    (void)cameraIndex;

    int poseOutputOrder = 0;
    if (m_pConfigManager) {
        poseOutputOrder = m_pConfigManager->GetConfigResult().poseOutputOrder;
    }

    int count = static_cast<int>(result.positions.size());
    std::string resultText = std::to_string(count);

    for (const auto& pos : result.positions) {
        double angle1 = 0.0;
        double angle2 = 0.0;
        double angle3 = 0.0;
        ReorderPoseAngles(pos.roll, pos.pitch, pos.yaw, poseOutputOrder, angle1, angle2, angle3);

        // 每组输出 12 个浮点：先接近点(X/Y/Z/A/B/C)，后目标点(X/Y/Z/A/B/C)，姿态相同
        char buf[512];
        std::snprintf(buf, sizeof(buf),
                      "_%.3f_%.3f_%.3f_%.3f_%.3f_%.3f_%.3f_%.3f_%.3f_%.3f_%.3f_%.3f/",
                      pos.approachX, pos.approachY, pos.approachZ, angle1, angle2, angle3,
                      pos.x, pos.y, pos.z, angle1, angle2, angle3);
        resultText += buf;
    }

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

    const 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;
    int poseOutputOrder = 0;
    if (m_pConfigManager) {
        poseOutputOrder = m_pConfigManager->GetConfigResult().poseOutputOrder;
    }

    if (!result.positions.empty()) {
        FillPoseRegisters(result.positions.front(), poseOutputOrder, registers);
        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;
    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;
    }

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