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

/**
 * @file TCPServerMethods.cpp
 * @brief DiscHolePosePresenter TCP/Modbus communication methods
 *
 * TCP 协议：
 *   - 请求:  D1_X_Y_Z_A_B_C  表示 disc hole 检测
 *   - 请求:  R1_X_Y_Z_A_B_C  表示 disc rack 检测
 *   - 响应:  count_X_Y_Z_A_B_C
 *            (6 个浮点：目标点 XYZ + 姿态 A/B/C)
 *
 * Modbus 协议（默认端口 5020）：
 *   - 触发寄存器地址 0，工作状态地址 1，机器人位姿地址 2 (12 regs)
 *   - 结果起始地址 14 (12 regs = 6 floats)
 */

#include "DiscHolePosePresenter.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;
// 结果寄存器：12 = 6 floats * 2 regs，目标点(6 floats)
constexpr uint16_t kModbusResultRegisterCount = 12;

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

// 检测类型对应的中文标签
const char* DetectionTypeLabel(DetectionType type)
{
    switch (type) {
    case DETECTION_TYPE_DISC_HOLE:
        return "盘孔";
    case DETECTION_TYPE_DISC_RACK:
        return "架子";
    default:
        return "未知";
    }
}

} // namespace

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

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

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

    // 每次只发送一个目标，取第一个
    if (!result.positions.empty()) {
        const auto& pos = result.positions.front();
        double angle1 = 0.0;
        double angle2 = 0.0;
        double angle3 = 0.0;
        ReorderPoseAngles(pos.roll, pos.pitch, pos.yaw, poseOutputOrder, angle1, angle2, angle3);

        // 输出 6 个浮点：目标点(X/Y/Z + A/B/C)
        // 使用 QString::arg 格式化
        resultText += QString("_%1_%2_%3_%4_%5_%6")
            .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);
    }

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

    m_pTCPServer->SendResult(resultText);
}

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

void DiscHolePosePresenter::_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 (%s), hasPose=%d\n",
              DetectionTypeLabel(m_currentDetectionType), hasPose ? 1 : 0);

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

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

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

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

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

void DiscHolePosePresenter::_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 DiscHolePosePresenter::_ReadRobotPoseFromModbus()
{
    RobotPose6D pose;
    return pose;
}

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

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

    m_pTCPServer = new DiscHolePoseTCPProtocol();

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

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

    // 连接检测触发信号
    // TCP 命令：D1_X_Y_Z_A_B_C 为 disc hole，R1_X_Y_Z_A_B_C 为 disc rack
    QObject::connect(m_pTCPServer, &DiscHolePoseTCPProtocol::DetectionTriggered,
                     this, [this](int cameraIndex, DetectionType detectionType, const RobotPose6D& robotPose) {
                         LOG_DEBUG("TCP triggered detection, cameraIndex: %d, type: %d (%s)\n",
                                   cameraIndex, static_cast<int>(detectionType),
                                   DetectionTypeLabel(detectionType));
                         TriggerDetection(cameraIndex, detectionType, 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 (DiscHolePose protocol: D1/R1 commands)\n", port);
    return 0;
}

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

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

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

    CheckAndUpdateWorkStatus();
}

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

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