d330viewer/src/core/NetworkManager.cpp

#include "core/NetworkManager.h"
#include "core/GVSPParser.h"
#include <QDebug>
#include <QVariant>

NetworkManager::NetworkManager(QObject *parent)
    : QObject(parent)
    , m_controlSocket(new QUdpSocket(this))
    , m_dataSocket(new QUdpSocket(this))
    , m_gvspParser(new GVSPParser(this))
    , m_controlPort(6790)
    , m_dataPort(3957)
    , m_isConnected(false)
{
    // 连接数据接收信号
    connect(m_dataSocket, &QUdpSocket::readyRead, this, &NetworkManager::onReadyRead);
    connect(m_dataSocket, &QUdpSocket::errorOccurred, this, &NetworkManager::onError);

    // 连接GVSP解析器信号
    connect(m_gvspParser, &GVSPParser::imageReceived, this, &NetworkManager::imageReceived);
    connect(m_gvspParser, &GVSPParser::leftImageReceived, this, &NetworkManager::leftImageReceived);
    connect(m_gvspParser, &GVSPParser::rightImageReceived, this, &NetworkManager::rightImageReceived);
    connect(m_gvspParser, &GVSPParser::rgbImageReceived, this, &NetworkManager::rgbImageReceived);
    connect(m_gvspParser, &GVSPParser::depthDataReceived, this, &NetworkManager::depthDataReceived);
    connect(m_gvspParser, &GVSPParser::pointCloudDataReceived, this, &NetworkManager::pointCloudDataReceived);
}

NetworkManager::~NetworkManager()
{
    disconnectFromCamera();
}

// ========== 连接和断开 ==========
bool NetworkManager::connectToCamera(const QString &ip, int controlPort, int dataPort)
{
    m_cameraIp = ip;
    m_controlPort = controlPort;
    m_dataPort = dataPort;

    // 绑定控制Socket到任意端口（让系统自动分配）
    if(!m_controlSocket->bind(QHostAddress::Any, 0)) {
        QString error = QString("Failed to bind control socket: %1")
                            .arg(m_controlSocket->errorString());
        qDebug() << error;
        emit errorOccurred(error);
        return false;
    }
    qDebug() << "Successfully bound control socket to port" << m_controlSocket->localPort();

    // 绑定数据接收端口
    if(!m_dataSocket->bind(QHostAddress::Any, m_dataPort)) {
        QString error = QString("Failed to bind data port %1: %2")
                            .arg(m_dataPort)
                            .arg(m_dataSocket->errorString());
        qDebug() << error;
        emit errorOccurred(error);
        return false;
    }

    // 设置UDP接收缓冲区大小为256MB（最大化，减少丢包）
    m_dataSocket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, QVariant(256 * 1024 * 1024));

    qDebug() << "Successfully bound data port" << m_dataPort;
    qDebug() << "Data socket state:" << m_dataSocket->state();
    qDebug() << "UDP receive buffer size:" << m_dataSocket->socketOption(QAbstractSocket::ReceiveBufferSizeSocketOption).toInt();

    m_isConnected = true;
    qDebug() << "Connected to camera:" << m_cameraIp << "Control port:" << m_controlPort << "Data port:" << m_dataPort;

    // Send STOP command to register client IP on camera
    sendStopCommand();
    qDebug() << "Sent STOP command to register client IP";

    emit connected();
    return true;
}

void NetworkManager::disconnectFromCamera()
{
    if(m_isConnected) {
        m_controlSocket->close();
        m_dataSocket->close();
        m_isConnected = false;
        qDebug() << "Disconnected from camera";
        emit disconnected();
    }
}

bool NetworkManager::isConnected() const
{
    return m_isConnected;
}

// ========== 发送控制命令 ==========
bool NetworkManager::sendCommand(const QString &command)
{
    if(!m_isConnected) {
        qDebug() << "Not connected to camera";
        return false;
    }

    QByteArray data = command.toUtf8();
    qDebug() << "Sending command to" << m_cameraIp << ":" << m_controlPort << "data:" << command;
    qDebug() << "Control socket state:" << m_controlSocket->state();
    qDebug() << "Control socket error:" << m_controlSocket->errorString();
    qDebug() << "Data to send (hex):" << data.toHex();
    qDebug() << "Data size:" << data.size();

    qint64 sent = m_controlSocket->writeDatagram(data, QHostAddress(m_cameraIp), m_controlPort);

    qDebug() << "writeDatagram returned:" << sent;
    if(sent == -1) {
        QString error = QString("Failed to send command: %1").arg(m_controlSocket->errorString());
        qDebug() << error;
        qDebug() << "Socket error code:" << m_controlSocket->error();
        emit errorOccurred(error);
        return false;
    }

    qDebug() << "Command sent successfully, bytes:" << sent;
    qDebug() << "Sent command:" << command;
    return true;
}

bool NetworkManager::sendStartCommand()
{
    return sendCommand("START");
}

bool NetworkManager::sendStopCommand()
{
    return sendCommand("STOP");
}

bool NetworkManager::sendExposureCommand(int exposureTime)
{
    QString exposureCommand = QString("EXPOSURE:%1").arg(exposureTime);
    return sendCommand(exposureCommand);
}

// ========== 传输开关命令 ==========
bool NetworkManager::sendEnableLeftIR()
{
    return sendCommand("ENABLE_LEFT");
}

bool NetworkManager::sendDisableLeftIR()
{
    return sendCommand("DISABLE_LEFT");
}

bool NetworkManager::sendEnableRightIR()
{
    return sendCommand("ENABLE_RIGHT");
}

bool NetworkManager::sendDisableRightIR()
{
    return sendCommand("DISABLE_RIGHT");
}

bool NetworkManager::sendEnableRGB()
{
    return sendCommand("ENABLE_RGB");
}

bool NetworkManager::sendDisableRGB()
{
    return sendCommand("DISABLE_RGB");
}

bool NetworkManager::sendMonocularMode()
{
    return sendCommand("MONOCULAR");
}

bool NetworkManager::sendBinocularMode()
{
    return sendCommand("BINOCULAR");
}

// ========== 槽函数 ==========
void NetworkManager::onReadyRead()
{
    static int packetCount = 0;

    while (m_dataSocket->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(m_dataSocket->pendingDatagramSize());

        QHostAddress sender;
        quint16 senderPort;
        m_dataSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);

        // 只打印前5个包的详细信息
        if(packetCount < 5) {
    // qDebug() << "[NetworkManager] Packet" << packetCount
    // << "from" << sender.toString() << ":" << senderPort
    // << "size:" << datagram.size() << "bytes";
        }
        packetCount++;

        // 将数据包传递给GVSP解析器
        m_gvspParser->parsePacket(datagram);

        // 仍然发出原始数据信号（用于调试）
        emit dataReceived(datagram);
    }

    // 每1000个包打印一次统计（减少日志量）
    if(packetCount % 1000 == 0) {
    // qDebug() << "[NetworkManager] Total packets received:" << packetCount;
    }
}

void NetworkManager::onError(QAbstractSocket::SocketError socketError)
{
    QString error = QString("Socket error: %1").arg(m_dataSocket->errorString());
    qDebug() << error;
    emit errorOccurred(error);
}