GrabBag/App/TireHolePose/TireHolePoseApp/Presenter/Src/TCPServerMethods.cpp

/**
 * @file TCPServerMethods.cpp
 * @brief TireHolePosePresenter TCP/Modbus communication methods
 *
 * TCP 协议（眼在手外）：
 *   - 请求:  D1                    触发相机1检测（无需机械臂位姿）
 *   - 响应:  count_X1_Y1_Z1_A1_B1_C1_R1_...  返回全部 count 个目标
 *            (每个目标 7 个浮点：XYZ + 姿态 A/B/C + 孔半径 R)
 *
 * Modbus 协议（默认端口 5020）：
 *   - 触发寄存器地址 0，工作状态地址 1，机器人位姿地址 2 (12 regs)
 *   - 结果起始地址 14 (14 regs = 7 floats: XYZ + 姿态 A/B/C + 孔半径 R)
 */

#include "TireHolePosePresenter.h"

#include <QApplication>

#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;
// 结果寄存器：14 = 7 floats * 2 regs，目标点(6 floats) + 孔半径(1 float)
constexpr uint16_t kModbusResultRegisterCount = 14;

constexpr uint16_t kDefaultModbusPort = 5020;

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）
    const float values[6] = {
        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 < 6; ++i) {
        FloatToRegisters(values[i], registers[i * 2], registers[i * 2 + 1]);
    }
}

} // namespace

// ============================================================================
// TCP 结果发送
// ============================================================================

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

    (void)cameraIndex;

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

    const int count = static_cast<int>(result.positions.size());
    QString resultText = QString::number(count);

    // 返回全部检测结果
    for (int i = 0; i < count; ++i) {
        const auto& pos = result.positions[i];
        double angle1 = 0.0;
        double angle2 = 0.0;
        double angle3 = 0.0;
        ReorderPoseAngles(pos.roll, pos.pitch, pos.yaw, poseOutputOrder, angle1, angle2, angle3);

        // 获取孔半径（优先从 tireHoleInfoList 获取，fallback 为 0）
        double holeR = 0.0;
        if (i < static_cast<int>(result.tireHoleInfoList.size())) {
            holeR = result.tireHoleInfoList[i].holeR;
        }

        resultText += QString("_%1_%2_%3_%4_%5_%6_%7")
            .arg(pos.x, 0, 'f', 3)
            .arg(pos.y, 0, 'f', 3)
            .arg(pos.z, 0, 'f', 3)
            .arg(angle1, 0, 'f', 3)
            .arg(angle2, 0, 'f', 3)
            .arg(angle3, 0, 'f', 3)
            .arg(holeR, 0, 'f', 3);
    }

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

    m_pTCPServer->SendResult(resultText);
}

// ============================================================================
// Modbus 结果发布
// ============================================================================

void TireHolePosePresenter::_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;

        // 追加输出孔半径 holeR（第 7 个 float，寄存器偏移 12-13）
        double holeR = 0.0;
        if (!result.tireHoleInfoList.empty()) {
            holeR = result.tireHoleInfoList.front().holeR;
        }
        FloatToRegisters(static_cast<float>(holeR), registers[12], registers[13]);
    }

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

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

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

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

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

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

void TireHolePosePresenter::_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 TireHolePosePresenter::_ReadRobotPoseFromModbus()
{
    RobotPose6D pose;
    auto regToFloat = [this](int offset) -> float {
        uint32_t raw = (static_cast<uint32_t>(m_modbusRobotPoseRegs[offset]) << 16)
                     | static_cast<uint32_t>(m_modbusRobotPoseRegs[offset + 1]);
        float val = 0;
        std::memcpy(&val, &raw, sizeof(val));
        return val;
    };
    pose.x = regToFloat(0);
    pose.y = regToFloat(2);
    pose.z = regToFloat(4);
    pose.a = regToFloat(6);
    pose.b = regToFloat(8);
    pose.c = regToFloat(10);
    return pose;
}

// ============================================================================
// TCP Server 生命周期
// ============================================================================

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

    m_pTCPServer = new TireHolePoseTCPProtocol();

    // 把 TCP 协议对象移到主线程，确保信号能通过事件循环正确投递
    if (QApplication::instance()) {
        m_pTCPServer->moveToThread(QApplication::instance()->thread());
    }

    // 连接 TCP 连接状态变化信号
    // 注意：必须传入 this (TireHolePosePresenter) 作为 context 对象，
    // 因为 InitTCPServer() 在工作线程中调用，而 Presenter 在主线程创建，
    // 不传 context 会导致 Qt 默认使用工作线程作为 receiver 线程，
    // 信号在主线程 emit 后被 QueuedConnection 投递到工作线程（无事件循环），永远不执行。
    QObject::connect(m_pTCPServer, &TireHolePoseTCPProtocol::ConnectionChanged,
                     this, [this](bool connected) {
                         OnTCPConnectionChanged(connected);
                     });

    // 连接检测触发信号
    // 眼在手外模式：TCP 命令 D1 为触发相机1检测
    QObject::connect(m_pTCPServer, &TireHolePoseTCPProtocol::DetectionTriggered,
                     this, [this](int cameraIndex, const RobotPose6D& robotPose) {
                         LOG_DEBUG("TCP triggered detection, cameraIndex: %d\n", cameraIndex);
                         TriggerDetection(cameraIndex, robotPose);
                     });

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

    if (!m_pTCPServer->StartServer(port)) {
        LOG_ERROR("Failed to start TCP server on port %d\n", port);
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
        return -1;
    }

    LOG_DEBUG("TCP server started on port %d (TireHolePose protocol: D1/R1 commands)\n", port);
    return 0;
}

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

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

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

    CheckAndUpdateWorkStatus();
}

// ============================================================================
// 对外发送接口
// ============================================================================

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