GrabBag/App/DroneScrewbolt/DroneScrewServer/DroneScrewZmqProtocol.cpp

#include "DroneScrewZmqProtocol.h"
#include "MjpegEncoder.h"
#include "VrLog.h"
#include "VrError.h"
#include "IWDRemoteReceiver.h"
#include "AuthManager.h"

// 复用 WDRemoteReceiver 头部协议（48 字节）
#include "IWDRemoteReceiver.h"

#include <QJsonObject>
#include <QJsonArray>
#include <QJsonDocument>
#include <QHostAddress>
#include <QUdpSocket>

#include <algorithm>
#include <cstdint>
#include <cstring>
#include <exception>
#include <mutex>
#include <new>
#include <vector>

namespace
{
constexpr int kRawImageZlibLevel = 9;
constexpr bool kUseFixedPrecisionMonoRawMjpeg = true;
constexpr int kFixedPrecisionMonoRawMjpegQuality = 25;
constexpr int kFixedPrecisionMonoRawScaleDiv = 4;

struct MjpegPayload
{
    std::vector<uint8_t> jpeg;
    uint32_t width{0};
    uint32_t height{0};
    uint32_t stride{0};
    uint32_t decodedSize{0};
    int quality{0};
    int scaleDiv{1};
};

void downscaleMono8Nearest(const uint8_t* src,
                           int srcWidth,
                           int srcHeight,
                           int scaleDiv,
                           std::vector<uint8_t>& dst,
                           int& dstWidth,
                           int& dstHeight)
{
    dstWidth = std::max(1, srcWidth / scaleDiv);
    dstHeight = std::max(1, srcHeight / scaleDiv);
    dst.resize(static_cast<size_t>(dstWidth) * static_cast<size_t>(dstHeight));
    for (int y = 0; y < dstHeight; ++y)
    {
        const int sy = std::min(srcHeight - 1, y * scaleDiv);
        const uint8_t* srcRow = src + static_cast<size_t>(sy) * static_cast<size_t>(srcWidth);
        uint8_t* dstRow = dst.data() + static_cast<size_t>(y) * static_cast<size_t>(dstWidth);
        for (int x = 0; x < dstWidth; ++x)
        {
            const int sx = std::min(srcWidth - 1, x * scaleDiv);
            dstRow[x] = srcRow[sx];
        }
    }
}

bool encodeMonoMjpegFixedRatio(const MvsImageData& left, MjpegPayload& out)
{
    if (!left.pData || left.width <= 0 || left.height <= 0 || left.dataSize == 0)
        return false;

    static MppMjpegEncoder s_mjpegEncoder;

    const int srcWidth = static_cast<int>(left.width);
    const int srcHeight = static_cast<int>(left.height);
    const int scaleDiv = kFixedPrecisionMonoRawScaleDiv;
    std::vector<uint8_t> scaled;
    const uint8_t* encodeData = left.pData;
    int encodeWidth = srcWidth;
    int encodeHeight = srcHeight;

    if (scaleDiv > 1)
    {
        downscaleMono8Nearest(left.pData, srcWidth, srcHeight,
                              scaleDiv, scaled, encodeWidth, encodeHeight);
        encodeData = scaled.data();
    }

    std::vector<uint8_t> jpeg;
    s_mjpegEncoder.SetQuality(kFixedPrecisionMonoRawMjpegQuality);
    const bool ok = s_mjpegEncoder.EncodeMono8(
        encodeData,
        static_cast<size_t>(encodeWidth) * static_cast<size_t>(encodeHeight),
        encodeWidth,
        encodeHeight,
        jpeg);

    LOG_DEBUG("[PUB-raw] mjpeg fixed frame=%llu ok=%d q=%d scale=1/%d size=%zu raw=%u\n",
              left.frameID, ok ? 1 : 0, kFixedPrecisionMonoRawMjpegQuality,
              scaleDiv, jpeg.size(), left.dataSize);
    if (!ok)
    {
        LOG_WARN("[PUB-raw] mjpeg fixed failed frame=%llu q=%d scale=1/%d encode=%dx%d raw=%u fallback=zlib\n",
                 left.frameID, kFixedPrecisionMonoRawMjpegQuality,
                 scaleDiv, encodeWidth, encodeHeight, left.dataSize);
        return false;
    }

    static int s_mjpegOkLogCnt = 0;
    ++s_mjpegOkLogCnt;
    if (s_mjpegOkLogCnt <= 5 || s_mjpegOkLogCnt % 30 == 0)
    {
        const double ratio = left.dataSize > 0
                                 ? static_cast<double>(jpeg.size()) / static_cast<double>(left.dataSize)
                                 : 0.0;
        const double mbpsAt2Fps = static_cast<double>(jpeg.size()) * 8.0 * 2.0 / 1000000.0;
        LOG_INFO("[PUB-raw] mjpeg fixed ok #%d frame=%llu q=%d scale=1/%d out=%dx%d jpeg=%zu raw=%u ratio=%.4f approx=%.2fMbps@2fps\n",
                 s_mjpegOkLogCnt, left.frameID, kFixedPrecisionMonoRawMjpegQuality,
                 scaleDiv, encodeWidth, encodeHeight, jpeg.size(), left.dataSize,
                 ratio, mbpsAt2Fps);
    }

    out.jpeg.swap(jpeg);
    out.width = static_cast<uint32_t>(encodeWidth);
    out.height = static_cast<uint32_t>(encodeHeight);
    out.stride = static_cast<uint32_t>(encodeWidth);
    out.decodedSize = static_cast<uint32_t>(
        static_cast<size_t>(encodeWidth) * static_cast<size_t>(encodeHeight));
    out.quality = kFixedPrecisionMonoRawMjpegQuality;
    out.scaleDiv = scaleDiv;
    return true;
}

const char* rawCompressionName(uint32_t compression)
{
    switch (compression)
    {
    case WD_REMOTE_IMAGE_COMPRESSION_RAW: return "raw";
    case WD_REMOTE_IMAGE_COMPRESSION_PNG: return "png";
    case WD_REMOTE_IMAGE_COMPRESSION_MJPEG: return "mjpeg";
    case WD_REMOTE_IMAGE_COMPRESSION_ZLIB: return "zlib";
    default: return "unknown";
    }
}

void fillDetectionJson(const DroneScrewResult& result, QJsonObject& obj)
{
    obj["frameId"]   = static_cast<qint64>(result.frameId);
    obj["ts"]        = static_cast<qint64>(result.timestampUs);
    obj["width"]     = result.imageWidth;
    obj["height"]    = result.imageHeight;
    obj["success"]   = result.success;
    obj["errorCode"] = result.errorCode;
    obj["msg"]       = QString::fromStdString(result.message);
    obj["isFinalResult"] = result.isFinalResult;
    obj["rankScore"] = result.rankScore;
    obj["rankSampleCount"] = result.rankSampleCount;
    obj["saveIndex"] = static_cast<qint64>(result.saveIndex);

    QJsonArray boxes;
    for (const auto& b : result.boxes)
    {
        QJsonObject jb;
        jb["cls"]   = b.classId;
        jb["score"] = b.score;
        jb["x"]     = b.x;
        jb["y"]     = b.y;
        jb["w"]     = b.width;
        jb["h"]     = b.height;
        if (b.hasPhysicalHeight)
        {
            const double heightMm = static_cast<double>(b.physicalHeightMm);
            jb["height_mm"] = heightMm;
            jb["hasStereoMatch"] = b.hasStereoMatch;
            jb["trusted"] = b.trusted;
            jb["matchConfidence"] = b.matchConfidence;
            jb["matchStatus"] = QString::fromStdString(b.matchStatus);
            jb["pairId"] = b.pairId;
            jb["leftIndex"] = b.leftIndex;
            jb["rightIndex"] = b.rightIndex;
        }
        boxes.append(jb);
    }
    obj["boxes"] = boxes;

    QJsonArray distances;
    for (const auto& d : result.distances)
    {
        QJsonObject jd;
        jd["fromId"] = d.fromId;
        jd["toId"] = d.toId;
        jd["distanceMm"] = static_cast<double>(d.distanceMm);
        distances.append(jd);
    }
    obj["distances"] = distances;
}

void setErrorResp(QJsonObject& resp, int code, const QString& msg)
{
    resp["ok"] = false;
    resp["code"] = code;
    resp["msg"] = msg;
}

void appendObject(QJsonObject& dst, const QJsonObject& src)
{
    for (auto it = src.constBegin(); it != src.constEnd(); ++it)
        dst[it.key()] = it.value();
}

QString localMachineCode()
{
    static QString code = QString::fromStdString(AuthManager::GetMachineCode());
    return code;
}

void fillServerInfoJson(const QString& cmd,
                        const QString& rtspUrl,
                        int controlPort,
                        int resultPort,
                        int rawImagePort,
                        const QJsonObject& req,
                        QJsonObject& resp)
{
    resp["ok"] = true;
    resp["code"] = 0;
    resp["cmd"] = cmd;
    resp["machineCode"] = localMachineCode();
    resp["clientMachineCode"] = req["machineCode"].toString();
    resp["deviceName"] = "DroneScrewServer";
    resp["rtsp"] = rtspUrl;
    resp["controlPort"] = controlPort;
    resp["resultPort"] = resultPort;
    resp["rawImagePort"] = rawImagePort;
}

int fallbackHandlerError()
{
    return ERR_CODE(DRONESCREW_ERR_ZMQ_HANDLER_NOT_SET);
}
}

DroneScrewZmqProtocol::DroneScrewZmqProtocol(QObject* parent)
    : QObject(parent)
{
}

DroneScrewZmqProtocol::~DroneScrewZmqProtocol()
{
    stopServer();
}

bool DroneScrewZmqProtocol::startServer(int controlPort, int resultPort, int rawImagePort)
{
    if (m_bRunning.load()) return true;

    m_controlPort  = controlPort;
    m_resultPort   = resultPort;
    m_rawImagePort = rawImagePort;

    // 1) REP 控制
    if (!IVrZeroMQServer::CreateObject(&m_pZmqServer) || !m_pZmqServer)
    {
        LOG_ERROR("ZmqServer create fail\n");
        return false;
    }

    auto onEvent = [](MQEvent ev) {
        LOG_DEBUG("Zmq Event: %d\n", (int)ev);
    };
    auto onRecv = [this](const char* data, const size_t len) -> std::string {
        QByteArray req(data, static_cast<int>(len));
        QByteArray resp = this->handleControlMessage(req);
        return std::string(resp.constData(), static_cast<size_t>(resp.size()));
    };

    int r = m_pZmqServer->Init(controlPort, onEvent, onRecv, false);
    if (r != 0)
    {
        LOG_ERROR("ZmqServer Init fail: %d\n", r);
        stopServer();
        return false;
    }

    // 2) PUB 检测结果 (topic="result")
    if (!IVrZeroMQPublisher::CreateObject(&m_pZmqPublisher) || !m_pZmqPublisher)
    {
        LOG_ERROR("ZmqPublisher create fail\n");
        stopServer();
        return false;
    }
    if (m_pZmqPublisher->Init(resultPort) != 0)
    {
        LOG_ERROR("ZmqPublisher init fail\n");
        stopServer();
        return false;
    }

    // 3) PUB 原始图像 (topic="raw")，可选
    if (rawImagePort > 0)
    {
        if (!IVrZeroMQPublisher::CreateObject(&m_pZmqRawPublisher) || !m_pZmqRawPublisher)
        {
            LOG_ERROR("ZmqRawPublisher create fail\n");
            stopServer();
            return false;
        }
        if (m_pZmqRawPublisher->Init(rawImagePort) != 0)
        {
            LOG_ERROR("ZmqRawPublisher init fail\n");
            stopServer();
            return false;
        }
    }

    if (!startDiscoveryServer())
    {
        LOG_ERROR("[UDP-DISCOVER] start fail port=%u\n",
                  static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT));
        stopServer();
        return false;
    }

    m_bRunning = true;
    LOG_INFO("ZMQ control=%d, result=%d, rawImage=%d, discoverUdp=%u\n",
             controlPort, resultPort, rawImagePort,
             static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT));
    return true;
}

void DroneScrewZmqProtocol::stopServer()
{
    stopDiscoveryServer();
    if (m_pZmqServer)        { m_pZmqServer->UnInit();        delete m_pZmqServer;        m_pZmqServer        = nullptr; }
    if (m_pZmqPublisher)     { m_pZmqPublisher->UnInit();     delete m_pZmqPublisher;     m_pZmqPublisher     = nullptr; }
    if (m_pZmqRawPublisher)  { m_pZmqRawPublisher->UnInit();  delete m_pZmqRawPublisher;  m_pZmqRawPublisher  = nullptr; }
    m_bRunning = false;
}

bool DroneScrewZmqProtocol::startDiscoveryServer()
{
    if (m_pDiscoverySocket) return true;

    m_pDiscoverySocket = new QUdpSocket(this);
    const bool ok = m_pDiscoverySocket->bind(
        QHostAddress::AnyIPv4,
        static_cast<quint16>(WD_REMOTE_DISCOVERY_PORT),
        QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
    if (!ok)
    {
        LOG_ERROR("[UDP-DISCOVER] bind failed port=%u err=%s\n",
                  static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT),
                  m_pDiscoverySocket->errorString().toStdString().c_str());
        delete m_pDiscoverySocket;
        m_pDiscoverySocket = nullptr;
        return false;
    }

    connect(m_pDiscoverySocket, &QUdpSocket::readyRead,
            this, &DroneScrewZmqProtocol::handleDiscoveryDatagrams);
    LOG_INFO("[UDP-DISCOVER] listen port=%u\n",
             static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT));
    return true;
}

void DroneScrewZmqProtocol::stopDiscoveryServer()
{
    if (!m_pDiscoverySocket) return;
    m_pDiscoverySocket->close();
    delete m_pDiscoverySocket;
    m_pDiscoverySocket = nullptr;
}

void DroneScrewZmqProtocol::handleDiscoveryDatagrams()
{
    while (m_pDiscoverySocket && m_pDiscoverySocket->hasPendingDatagrams())
    {
        QByteArray req;
        req.resize(static_cast<int>(m_pDiscoverySocket->pendingDatagramSize()));

        QHostAddress sender;
        quint16 senderPort = 0;
        const qint64 readBytes =
            m_pDiscoverySocket->readDatagram(req.data(), req.size(),
                                             &sender, &senderPort);
        if (readBytes <= 0)
            continue;
        req.resize(static_cast<int>(readBytes));

        QJsonParseError err;
        const QJsonDocument doc = QJsonDocument::fromJson(req, &err);
        if (err.error != QJsonParseError::NoError || !doc.isObject())
        {
            LOG_DEBUG("[UDP-DISCOVER] ignore invalid json from=%s:%u err=%s\n",
                      sender.toString().toStdString().c_str(),
                      static_cast<unsigned>(senderPort),
                      err.errorString().toStdString().c_str());
            continue;
        }

        const QJsonObject obj = doc.object();
        const QString cmd = obj["cmd"].toString();
        if (cmd != "discover")
            continue;

        QJsonObject resp;
        fillServerInfoJson("discover", m_rtspUrl, m_controlPort, m_resultPort,
                           m_rawImagePort, obj, resp);
        if (m_serverInfoProvider)
            appendObject(resp, m_serverInfoProvider());

        const QByteArray payload =
            QJsonDocument(resp).toJson(QJsonDocument::Compact);
        const qint64 sent =
            m_pDiscoverySocket->writeDatagram(payload, sender, senderPort);
        LOG_DEBUG("[UDP-DISCOVER] reply %s:%u bytes=%lld reqMachine=%s\n",
                  sender.toString().toStdString().c_str(),
                  static_cast<unsigned>(senderPort),
                  static_cast<long long>(sent),
                  obj["machineCode"].toString().toStdString().c_str());
    }
}

QByteArray DroneScrewZmqProtocol::handleControlMessage(const QByteArray& reqData)
{
    LOG_DEBUG("[ZMQ-REP] recv: %s\n", reqData.constData());

    QJsonParseError err;
    QJsonDocument doc = QJsonDocument::fromJson(reqData, &err);
    if (err.error != QJsonParseError::NoError || !doc.isObject())
    {
        LOG_DEBUG("[ZMQ-REP] parse error: %s\n", err.errorString().toStdString().c_str());
        QJsonObject o;
        setErrorResp(o, ERR_CODE(DRONESCREW_ERR_INVALID_JSON), "invalid json");
        return QJsonDocument(o).toJson(QJsonDocument::Compact);
    }
    QJsonObject obj = doc.object();
    QString cmd = obj["cmd"].toString();
    LOG_DEBUG("[ZMQ-REP] cmd=%s\n", cmd.toStdString().c_str());

    QJsonObject resp;
    resp["ok"]   = true;
    resp["code"] = 0;
    resp["cmd"]  = cmd;

    if (cmd == "start")
    {
        // 解析检测模式（mono/binocular），设置后才启动检测
        const std::string detectMode = obj["detectMode"].toString("binocular").toStdString();
        emit detectModeRequested(detectMode);

        const bool enableRaw = (m_rawImagePort > 0 &&
                                m_pZmqRawPublisher != nullptr);
        emit rawImagePublishEnabledRequested(enableRaw);
        const int ret = m_startWorkHandler ? m_startWorkHandler()
                                           : fallbackHandlerError();
        resp["mode"] = "detection";
        resp["detectMode"] = QString::fromStdString(detectMode);
        resp["rawImagePublish"] = enableRaw;
        if (ret != 0)
        {
            emit rawImagePublishEnabledRequested(false);
            setErrorResp(resp, ret, "start detection failed");
        }
    }
    else if (cmd == "set_detect_mode")
    {
        const QString mode = obj["detectMode"].toString(obj["mode"].toString());
        if (mode != "mono" && mode != "binocular")
        {
            setErrorResp(resp, ERR_CODE(DEV_ARG_INVAILD), "invalid detectMode");
        }
        else
        {
            emit detectModeRequested(mode.toStdString());
            resp["detectMode"] = mode;
        }
    }
    else if (cmd == "stop")
    {
        emit rawImagePublishEnabledRequested(false);
        const int ret = m_stopWorkHandler ? m_stopWorkHandler()
                                          : fallbackHandlerError();
        if (ret != 0)
        {
            setErrorResp(resp, ret, "stop detection failed");
        }
        else if (m_finalDetectionResultProvider)
        {
            DroneScrewResult finalResult;
            if (m_finalDetectionResultProvider(finalResult))
            {
                finalResult.isFinalResult = true;
                fillDetectionJson(finalResult, resp);
                resp["hasFinalResult"] = true;
                publishDetectionResult(finalResult);
            }
            else
            {
                resp["hasFinalResult"] = false;
            }
        }
    }
    else if (cmd == "start_stream")
    {
        emit rawImagePublishEnabledRequested(false);
        const int ret = m_startLiveStreamHandler ? m_startLiveStreamHandler()
                                                 : fallbackHandlerError();
        resp["mode"] = "live_stream";
        resp["rawImagePublish"] = false;
        if (ret != 0)
            setErrorResp(resp, ret, "start live stream failed");
    }
    else if (cmd == "stop_stream")
    {
        const int ret = m_stopLiveStreamHandler ? m_stopLiveStreamHandler()
                                                : fallbackHandlerError();
        resp["mode"] = "idle";
        if (ret != 0)
            setErrorResp(resp, ret, "stop live stream failed");
    }
    else if (cmd == "swap_cameras" || cmd == "swap_left_right")
    {
        const int ret = m_swapCameraRolesHandler ? m_swapCameraRolesHandler()
                                                 : fallbackHandlerError();
        if (ret != 0)
        {
            setErrorResp(resp, ret, "swap camera roles failed");
        }
        else if (m_serverInfoProvider)
        {
            appendObject(resp, m_serverInfoProvider());
        }
    }
    else if (cmd == "single")
    {
        if (!m_singleDetectionHandler)
        {
            setErrorResp(resp, ERR_CODE(DRONESCREW_ERR_SINGLE_HANDLER_NOT_SET),
                         "single handler not set");
        }
        else
        {
            std::unique_lock<std::mutex> singleLock(m_singleMutex, std::try_to_lock);
            if (!singleLock.owns_lock())
            {
                setErrorResp(resp, ERR_CODE(DRONESCREW_ERR_SINGLE_BUSY),
                             "single detection busy");
            }
            else
            {
                DroneScrewResult result = m_singleDetectionHandler();
                fillDetectionJson(result, resp);
                resp["ok"] = result.success;
                resp["code"] = result.success ? 0
                                              : (result.errorCode != 0
                                                     ? result.errorCode
                                                     : ERR_CODE(DRONESCREW_ERR_RESPONSE_NOT_OK));
            }
        }
    }
    else if (cmd == "open")
    {
        const QString targetMachineCode = obj["targetMachineCode"].toString();
        const QString selfMachineCode = localMachineCode();
        if (!targetMachineCode.isEmpty() && targetMachineCode != selfMachineCode)
        {
            setErrorResp(resp, ERR_CODE(DRONESCREW_ERR_MACHINE_CODE_MISMATCH),
                         "machine code mismatch");
            resp["machineCode"] = selfMachineCode;
        }
        else
        {
            fillServerInfoJson(cmd, m_rtspUrl, m_controlPort, m_resultPort,
                               m_rawImagePort, obj, resp);
            if (m_serverInfoProvider)
                appendObject(resp, m_serverInfoProvider());
        }
    }
    else if (cmd == "set_exposure")
    {
        emit setExposureRequested(obj["value"].toDouble());
    }
    else if (cmd == "set_gain")
    {
        emit setGainRequested(obj["value"].toDouble());
    }
    else if (cmd == "set_algo_params")
    {
        DroneScrewAlgoParams p;
        p.scoreThreshold = static_cast<float>(obj["score"].toDouble(0.5));
        p.nmsThreshold   = static_cast<float>(obj["nms"].toDouble(0.45));
        p.inputWidth     = obj["width"].toInt(4096);
        p.inputHeight    = obj["height"].toInt(3000);
        const QString configPath = obj.contains("configPath")
                                       ? obj["configPath"].toString()
                                       : (obj.contains("modelPath")
                                              ? obj["modelPath"].toString()
                                              : obj["model"].toString());
        p.modelPath      = configPath.toStdString();
        p.modelType      = obj["modelType"].toInt(1);
        p.expectedBoltCount = obj.contains("expected")
                                  ? obj["expected"].toInt(8)
                                  : obj["expectedBoltCount"].toInt(8);
        emit updateAlgoParamsRequested(p);
    }
    else if (cmd == "get_info")
    {
        fillServerInfoJson(cmd, m_rtspUrl, m_controlPort, m_resultPort,
                           m_rawImagePort, obj, resp);
        if (m_serverInfoProvider)
            appendObject(resp, m_serverInfoProvider());
        if (m_calibrationInfoProvider)
            resp["calibration"] = m_calibrationInfoProvider();
    }
    else
    {
        setErrorResp(resp, ERR_CODE(DRONESCREW_ERR_UNKNOWN_CMD), "unknown cmd");
    }
    QByteArray rspData = QJsonDocument(resp).toJson(QJsonDocument::Compact);
    const int previewSize = std::min(rspData.size(), 512);
    LOG_DEBUG("[ZMQ-REP] send resp bytes=%d preview=%.*s%s\n",
              rspData.size(), previewSize, rspData.constData(),
              rspData.size() > previewSize ? "..." : "");
    return rspData;
}

void DroneScrewZmqProtocol::publishDetectionResult(const DroneScrewResult& result)
{
    try
    {
        if (!m_pZmqPublisher) { LOG_DEBUG("[PUB-result] no publisher\n"); return; }

        QJsonObject obj;
        fillDetectionJson(result, obj);

        QByteArray payload = QJsonDocument(obj).toJson(QJsonDocument::Compact);

        std::lock_guard<std::mutex> lk(m_pubMutex);
        const int ret = m_pZmqPublisher->Publish("result", payload.constData(),
                                                 static_cast<size_t>(payload.size()));
        static int pubCnt = 0;
        static int pubFailCnt = 0;
        if (ret != 0)
        {
            ++pubFailCnt;
            LOG_ERROR("[PUB-result] publish failed ret=%d fail=%d frame=%llu payload=%d\n",
                      ret, pubFailCnt, result.frameId, payload.size());
            return;
        }
        if (++pubCnt % 100 == 0)
            LOG_DEBUG("[PUB-result] #%d frame=%llu boxes=%zu\n", pubCnt, result.frameId, result.boxes.size());
    }
    catch (const std::exception& e)
    {
        LOG_ERROR("[PUB-result] exception frame=%llu boxes=%zu: %s\n",
                  result.frameId, result.boxes.size(), e.what());
    }
    catch (...)
    {
        LOG_ERROR("[PUB-result] unknown exception frame=%llu boxes=%zu\n",
                  result.frameId, result.boxes.size());
    }
}

// MvsImageData.pixelFormat (Hik 原值，enMvGvspPixelType 透传) → WDRemotePixelFormat
// 常见映射，未知归 UNKNOWN
static uint32_t mvsPixelFormatToWDRemote(int mvsPixelFormat)
{
    switch (mvsPixelFormat)
    {
    case 0x01080001: return static_cast<uint32_t>(WDRemotePixelFormat::MONO8);    // PixelType_Gvsp_Mono8
    case 0x02180014: return static_cast<uint32_t>(WDRemotePixelFormat::BGR8);     // PixelType_Gvsp_BGR8_Packed
    case 0x02180015: return static_cast<uint32_t>(WDRemotePixelFormat::RGB8);     // PixelType_Gvsp_RGB8_Packed
    case 0x01080009: return static_cast<uint32_t>(WDRemotePixelFormat::BAYERRG8); // PixelType_Gvsp_BayerRG8
    default:         return static_cast<uint32_t>(WDRemotePixelFormat::UNKNOWN);
    }
}

void DroneScrewZmqProtocol::publishRawImage(const MvsImageData& left, const MvsImageData& right)
{
    try
    {
        if (!m_pZmqRawPublisher) return;
        // 左目必须有效；右目允许缺失（缺失时右目字段保持 0，接收端按空右目处理）
        if (!left.pData || left.dataSize == 0) return;

        LOG_DEBUG("[PUB-raw] enter frame=%llu left=%ux%u right=%ux%u\n",
                  left.frameID, left.width, left.height, right.width, right.height);

        WDRemoteBinocularRawImageHeader hdr{};
        hdr.magic       = WD_REMOTE_BINOCULAR_RAW_IMAGE_MAGIC;
        hdr.version     = WD_REMOTE_BINOCULAR_RAW_IMAGE_VERSION;
        hdr.frameId     = left.frameID;
        hdr.timestampUs = static_cast<int64_t>(left.timestamp);

        // 左目（多数 GVSP 单字节像素 stride==width）
        hdr.leftWidth       = static_cast<uint32_t>(left.width);
        hdr.leftHeight      = static_cast<uint32_t>(left.height);
        hdr.leftStride      = static_cast<uint32_t>(left.width);
        hdr.leftPixelFormat = mvsPixelFormatToWDRemote(left.pixelFormat);
        hdr.reserved[3]     = static_cast<uint32_t>(left.width);
        hdr.reserved[4]     = static_cast<uint32_t>(left.height);
        hdr.reserved[5]     = 1u;

        // 右目（无效时保持 0）
        const bool hasRight = (right.pData != nullptr && right.dataSize > 0);
        if (hasRight)
        {
            hdr.rightWidth       = static_cast<uint32_t>(right.width);
            hdr.rightHeight      = static_cast<uint32_t>(right.height);
            hdr.rightStride      = static_cast<uint32_t>(right.width);
            hdr.rightPixelFormat = mvsPixelFormatToWDRemote(right.pixelFormat);
        }

        MjpegPayload leftMjpeg;
        bool useMjpeg = false;
        const bool singleRawFrame = !hasRight;
        if (kUseFixedPrecisionMonoRawMjpeg &&
            singleRawFrame &&
            hdr.leftPixelFormat == static_cast<uint32_t>(WDRemotePixelFormat::MONO8))
        {
            useMjpeg = encodeMonoMjpegFixedRatio(left, leftMjpeg);
        }

        QByteArray leftZ;
        QByteArray rightZ;
        bool useZlib = false;
        if (!useMjpeg)
        {
            LOG_DEBUG("[PUB-raw] before zlib encode frame=%llu level=%d\n",
                      left.frameID, kRawImageZlibLevel);

            leftZ = qCompress(reinterpret_cast<const uchar*>(left.pData),
                              static_cast<int>(left.dataSize), kRawImageZlibLevel);
            useZlib = !leftZ.isEmpty();
            if (useZlib && hasRight)
            {
                rightZ = qCompress(reinterpret_cast<const uchar*>(right.pData),
                                   static_cast<int>(right.dataSize), kRawImageZlibLevel);
                useZlib = !rightZ.isEmpty();
            }

            LOG_DEBUG("[PUB-raw] after zlib encode frame=%llu useZlib=%d leftSize=%d rightSize=%d\n",
                      left.frameID, useZlib ? 1 : 0, leftZ.size(), rightZ.size());
        }

        if (useMjpeg)
        {
            hdr.reserved[0]   = WD_REMOTE_IMAGE_COMPRESSION_MJPEG;
            hdr.reserved[1]   = leftMjpeg.decodedSize;
            hdr.reserved[2]   = 0u;
            hdr.leftWidth     = leftMjpeg.width;
            hdr.leftHeight    = leftMjpeg.height;
            hdr.leftStride    = leftMjpeg.stride;
            hdr.leftDataSize  = static_cast<uint32_t>(leftMjpeg.jpeg.size());
            hdr.rightDataSize = 0u;
            hdr.reserved[5]   = static_cast<uint32_t>(leftMjpeg.scaleDiv);
        }
        else if (useZlib)
        {
            hdr.reserved[0]   = WD_REMOTE_IMAGE_COMPRESSION_ZLIB;
            hdr.reserved[1]   = static_cast<uint32_t>(left.dataSize);
            hdr.reserved[2]   = hasRight ? static_cast<uint32_t>(right.dataSize) : 0u;
            hdr.leftDataSize  = static_cast<uint32_t>(leftZ.size());
            hdr.rightDataSize = hasRight ? static_cast<uint32_t>(rightZ.size()) : 0u;
        }
        else
        {
            hdr.reserved[0]   = WD_REMOTE_IMAGE_COMPRESSION_RAW;
            hdr.leftDataSize  = static_cast<uint32_t>(left.dataSize);
            hdr.rightDataSize = hasRight ? static_cast<uint32_t>(right.dataSize) : 0u;
        }

#if 0
        // Precision raw image publishing must be lossless; mono frames use zlib/raw too.

        LOG_DEBUG("[PUB-raw] before zlib encode frame=%llu level=%d\n",
                  left.frameID, kRawImageZlibLevel);

        // 真·无损：Qt qCompress（zlib/deflate 封装，bit-exact，跨平台、无需额外依赖）。
        // level=9 高压缩：优先降低精确检测全分辨率 raw 图像带宽，代价是 CPU 时间增加。
        QByteArray leftZ = qCompress(reinterpret_cast<const uchar*>(left.pData),
                                     static_cast<int>(left.dataSize), kRawImageZlibLevel);
        QByteArray rightZ;
        bool useZlib = !leftZ.isEmpty();
        if (useZlib && hasRight)
        {
            rightZ = qCompress(reinterpret_cast<const uchar*>(right.pData),
                               static_cast<int>(right.dataSize), kRawImageZlibLevel);
            useZlib = !rightZ.isEmpty();
        }

        LOG_DEBUG("[PUB-raw] after zlib encode frame=%llu useZlib=%d leftSize=%d rightSize=%d\n",
                  left.frameID, useZlib ? 1 : 0, leftZ.size(), rightZ.size());

        if (useZlib)
        {
            hdr.reserved[0]   = WD_REMOTE_IMAGE_COMPRESSION_ZLIB;
            hdr.reserved[1]   = static_cast<uint32_t>(left.dataSize);   // 解压后大小（左）
            hdr.reserved[2]   = hasRight ? static_cast<uint32_t>(right.dataSize) : 0u;  // 解压后大小（右）
            hdr.leftDataSize  = static_cast<uint32_t>(leftZ.size());
            hdr.rightDataSize = hasRight ? static_cast<uint32_t>(rightZ.size()) : 0u;
        }
        else
        {
            hdr.reserved[0]   = WD_REMOTE_IMAGE_COMPRESSION_RAW;
            hdr.leftDataSize  = static_cast<uint32_t>(left.dataSize);
            hdr.rightDataSize = hasRight ? static_cast<uint32_t>(right.dataSize) : 0u;
        }

#endif

        QByteArray payload;
        payload.reserve(static_cast<int>(sizeof(hdr) + hdr.leftDataSize + hdr.rightDataSize));
        payload.append(reinterpret_cast<const char*>(&hdr), sizeof(hdr));
        if (useMjpeg)
        {
            payload.append(reinterpret_cast<const char*>(leftMjpeg.jpeg.data()),
                           static_cast<int>(leftMjpeg.jpeg.size()));
        }
        else if (useZlib)
        {
            payload.append(leftZ);
            if (hasRight)
                payload.append(rightZ);
        }
        else
        {
            payload.append(reinterpret_cast<const char*>(left.pData), static_cast<int>(left.dataSize));
            if (hasRight)
                payload.append(reinterpret_cast<const char*>(right.pData), static_cast<int>(right.dataSize));
        }

        LOG_DEBUG("[PUB-raw] before ZMQ publish frame=%llu payloadSize=%d\n",
                  left.frameID, payload.size());

        std::lock_guard<std::mutex> lk(m_rawMutex);
        const int ret = m_pZmqRawPublisher->Publish("raw", payload.constData(),
                                                    static_cast<size_t>(payload.size()));

        LOG_DEBUG("[PUB-raw] after ZMQ publish frame=%llu ret=%d\n", left.frameID, ret);

        static int rawPubCnt = 0;
        static int rawFailCnt = 0;
        if (ret != 0)
        {
            ++rawFailCnt;
            LOG_ERROR("[PUB-raw] publish failed ret=%d fail=%d frame=%llu compression=%s payload=%d\n",
                      ret, rawFailCnt, left.frameID,
                      rawCompressionName(hdr.reserved[0]), payload.size());
            return;
        }
        ++rawPubCnt;
        if (rawPubCnt == 1)
            LOG_INFO("[PUB-raw] first frame=%llu compression=%s left=%ux%u fmt=%u size=%u raw=%u right=%ux%u fmt=%u size=%u raw=%u payload=%d\n",
                     left.frameID,
                     rawCompressionName(hdr.reserved[0]),
                     hdr.leftWidth, hdr.leftHeight, hdr.leftPixelFormat, hdr.leftDataSize,
                     hdr.reserved[0] != WD_REMOTE_IMAGE_COMPRESSION_RAW ? hdr.reserved[1] : hdr.leftDataSize,
                     hdr.rightWidth, hdr.rightHeight, hdr.rightPixelFormat, hdr.rightDataSize,
                     hdr.reserved[0] != WD_REMOTE_IMAGE_COMPRESSION_RAW ? hdr.reserved[2] : hdr.rightDataSize,
                     payload.size());
        else if (rawPubCnt % 100 == 0)
            LOG_DEBUG("[PUB-raw] #%d frame=%llu compression=%s left=%ux%u right=%ux%u payload=%d\n",
                      rawPubCnt, left.frameID, rawCompressionName(hdr.reserved[0]), left.width, left.height,
                      hasRight ? right.width : 0, hasRight ? right.height : 0,
                      payload.size());

        LOG_DEBUG("[PUB-raw] exit frame=%llu\n", left.frameID);
    }
    catch (const std::bad_alloc& e)
    {
        LOG_ERROR("[PUB-raw] memory allocation failed frame=%llu left=%ux%u leftSize=%u right=%ux%u rightSize=%u: %s\n",
                  left.frameID, left.width, left.height, left.dataSize,
                  right.width, right.height, right.dataSize, e.what());
    }
    catch (const std::exception& e)
    {
        LOG_ERROR("[PUB-raw] exception frame=%llu left=%ux%u leftSize=%u right=%ux%u rightSize=%u: %s\n",
                  left.frameID, left.width, left.height, left.dataSize,
                  right.width, right.height, right.dataSize, e.what());
    }
    catch (...)
    {
        LOG_ERROR("[PUB-raw] unknown exception frame=%llu left=%ux%u leftSize=%u right=%ux%u rightSize=%u\n",
                  left.frameID, left.width, left.height, left.dataSize,
                  right.width, right.height, right.dataSize);
    }
}