GrabBag/GrabBagApp/Presenter/Src/SerialProtocol.cpp

#include "SerialProtocol.h"
#include "VrLog.h"
#include "VrError.h"
#include <QStringList>
#include <QDebug>
#include <QThread>
#include <sstream>
#include <iomanip>
#include <chrono>
#include <thread>

#include <QSerialPortInfo>

// 协议常量定义
const QString SerialProtocol::PROTOCOL_START = "$";
const QString SerialProtocol::PROTOCOL_END = "#";
const QString SerialProtocol::CMD_START = "START";
const QString SerialProtocol::CMD_DATA = "DATA";
const QString SerialProtocol::SEPARATOR = ",";

SerialProtocol::SerialProtocol(QObject* parent)
    : QObject(parent)
    , m_serialStatus(STATUS_DISCONNECTED)
    , m_threadRunning(false)
{
    m_pSerialPort = new QSerialPort(this);
    
    // 连接信号和槽（保留原有的信号槽机制作为备用）
    connect(m_pSerialPort, &QSerialPort::readyRead, this, &SerialProtocol::OnSerialDataReceived);
    connect(m_pSerialPort, &QSerialPort::errorOccurred, this, &SerialProtocol::OnSerialError);
}

SerialProtocol::~SerialProtocol()
{
    // 先停止接收线程
    StopReceiveThread();
    // 再关闭串口
    CloseSerial();
}

int SerialProtocol::OpenSerial(const std::string& portName, int baudRate, 
                               int dataBits, int stopBits, int parity, int flowControl)
{
    if (m_pSerialPort->isOpen()) {
        LOG_WARNING("Serial port is already open: %s\n", portName.c_str());
        return SUCCESS;
    }
    
    LOG_INFO("Opening serial port: %s, baudRate: %d, dataBits: %d, stopBits: %d, parity: %d, flowControl: %d\n",
             portName.c_str(), baudRate, dataBits, stopBits, parity, flowControl);
    
    // 设置串口参数
    m_pSerialPort->setPortName(QString::fromStdString(portName));
    m_pSerialPort->setBaudRate(baudRate);
    
    // 设置数据位
    switch (dataBits) {
        case 5: m_pSerialPort->setDataBits(QSerialPort::Data5); break;
        case 6: m_pSerialPort->setDataBits(QSerialPort::Data6); break;
        case 7: m_pSerialPort->setDataBits(QSerialPort::Data7); break;
        case 8: m_pSerialPort->setDataBits(QSerialPort::Data8); break;
        default: m_pSerialPort->setDataBits(QSerialPort::Data8); break;
    }
    
    // 设置停止位
    switch (stopBits) {
        case 1: m_pSerialPort->setStopBits(QSerialPort::OneStop); break;
        case 2: m_pSerialPort->setStopBits(QSerialPort::TwoStop); break;
        default: m_pSerialPort->setStopBits(QSerialPort::OneStop); break;
    }
    
    // 设置校验位
    switch (parity) {
        case 0: m_pSerialPort->setParity(QSerialPort::NoParity); break;
        case 1: m_pSerialPort->setParity(QSerialPort::OddParity); break;
        case 2: m_pSerialPort->setParity(QSerialPort::EvenParity); break;
        default: m_pSerialPort->setParity(QSerialPort::NoParity); break;
    }
    
    // 设置流控制
    switch (flowControl) {
        case 0: m_pSerialPort->setFlowControl(QSerialPort::NoFlowControl); break;
        case 1: m_pSerialPort->setFlowControl(QSerialPort::HardwareControl); break;
        case 2: m_pSerialPort->setFlowControl(QSerialPort::SoftwareControl); break;
        default: m_pSerialPort->setFlowControl(QSerialPort::NoFlowControl); break;
    }
    
    // 尝试打开串口
    if (!m_pSerialPort->open(QIODevice::ReadWrite)) {
        LOG_ERROR("Failed to open serial port: %s, error: %s\n",   portName.c_str(), m_pSerialPort->errorString().toStdString().c_str());
        m_serialStatus = STATUS_ERROR;
        return ERR_CODE(DEV_OPEN_ERR);
    }
    
    // 清空接收缓冲区
    m_receiveBuffer.clear();
    m_pSerialPort->clear();
    
    // 设置读取超时和缓冲区大小
    m_pSerialPort->setReadBufferSize(1024);
    
    m_serialStatus = STATUS_CONNECTED;
    LOG_INFO("Serial port opened successfully: %s\n", portName.c_str());
    
    // 启动接收线程
    StartReceiveThread();
    
    // 触发连接状态回调
    if (m_connectionCallback) {
        m_connectionCallback(true);
    }
    
    return SUCCESS;
}

void SerialProtocol::CloseSerial()
{
    if (m_pSerialPort && m_pSerialPort->isOpen()) {
        LOG_INFO("Closing serial port: %s\n", m_pSerialPort->portName().toStdString().c_str());
        
        // 先停止接收线程
        StopReceiveThread();
        
        // 再关闭串口
        m_pSerialPort->close();
        m_serialStatus = STATUS_DISCONNECTED;
        
        // 触发连接状态回调
        if (m_connectionCallback) {
            m_connectionCallback(false);
        }
    }
}

int SerialProtocol::SendMultiTargetData(const MultiTargetData& multiTargetData, uint16_t cameraId)
{
    if (!m_pSerialPort || !m_pSerialPort->isOpen()) {
        LOG_ERROR("Serial port is not open, cannot send data\n");
        return -1;
    }
    
    LOG_DEBUG("Sending multi-target data, count: %d, camera ID: %d\n", multiTargetData.count, cameraId);
    
    // 构造协议数据
    std::string protocolData = BuildDataProtocol(multiTargetData, cameraId);
    
    // 发送数据
    int result = SendData(protocolData);
    if (result == SUCCESS) {
        LOG_INFO("Multi-target data sent successfully: %s\n", protocolData.c_str());
    } else {
        LOG_ERROR("Failed to send multi-target data\n");
    }
    
    return result;
}

SerialProtocol::SerialStatus SerialProtocol::GetSerialStatus() const
{
    return m_serialStatus;
}

void SerialProtocol::SetConnectionCallback(const ConnectionCallback& callback)
{
    m_connectionCallback = callback;
}

void SerialProtocol::SetWorkSignalCallback(const WorkSignalCallback& callback)
{
    m_workSignalCallback = callback;
}

bool SerialProtocol::IsOpen() const
{
    return m_pSerialPort && m_pSerialPort->isOpen();
}

void SerialProtocol::OnSerialDataReceived()
{
    // 注意：这个方法现在主要作为Qt信号槽的备用机制
    // 主要的数据接收由独立线程处理
    LOG_DEBUG("[Signal] OnSerialDataReceived() called (backup mechanism)\n");
    
    // 如果线程正在运行，优先使用线程处理
    if (m_threadRunning.load()) {
        LOG_DEBUG("[Signal] Thread is running, skipping signal-based processing\n");
        return;
    }
    
    if (!m_pSerialPort) {
        LOG_ERROR("[Signal] Serial port is null in OnSerialDataReceived\n");
        return;
    }
    
    if (!m_pSerialPort->isOpen()) {
        LOG_ERROR("[Signal] Serial port is not open in OnSerialDataReceived\n");
        return;
    }
    
    // 检查可用字节数
    qint64 bytesAvailable = m_pSerialPort->bytesAvailable();
    LOG_INFO("[Signal] Bytes available to read: %lld\n", bytesAvailable);
    
    if (bytesAvailable <= 0) {
        LOG_WARNING("[Signal] No bytes available but readyRead signal was triggered\n");
        return;
    }
    
    // 读取所有可用数据
    QByteArray data = m_pSerialPort->readAll();
    QString receivedData = QString::fromUtf8(data);
    
    LOG_INFO("[Signal] Actually read %d bytes from serial port\n", data.size());
    
    if (data.isEmpty()) {
        LOG_WARNING("[Signal] readAll() returned empty data\n");
        return;
    }
    
    // 使用互斥锁保护缓冲区
    {
        std::lock_guard<std::mutex> lock(m_bufferMutex);
        m_receiveBuffer += receivedData;
    }
    
    // 打印接收到的数据
    LOG_INFO("[Signal] Received [%d bytes]: \"%s\"\n", data.size(), receivedData.toStdString().c_str());
    
    // 如果包含不可打印字符，额外打印ASCII码
    bool hasNonPrintable = false;
    for (char c : data) {
        if (c < 32 || c > 126) {
            hasNonPrintable = true;
            break;
        }
    }
    
    if (hasNonPrintable) {
        QString asciiInfo = "ASCII: ";
        for (char c : data) {
            if (c >= 32 && c <= 126) {
                asciiInfo += QString("'%1' ").arg(c);
            } else {
                asciiInfo += QString("[%1] ").arg((unsigned char)c);
            }
        }
        LOG_INFO("[Signal] %s\n", asciiInfo.toStdString().c_str());
    }
    
    // 处理接收到的数据
    ProcessReceivedData();
}

void SerialProtocol::OnSerialError(QSerialPort::SerialPortError error)
{
    if (error == QSerialPort::NoError) {
        return;
    }
    
    QString errorString = m_pSerialPort ? m_pSerialPort->errorString() : "Unknown error";
    LOG_ERROR("=== SERIAL PORT ERROR ===\n");
    LOG_ERROR("Error type: %d\n", (int)error);
    LOG_ERROR("Error message: %s\n", errorString.toStdString().c_str());
    LOG_ERROR("Port open status: %s\n", (m_pSerialPort && m_pSerialPort->isOpen()) ? "OPEN" : "CLOSED");
    
    // 详细的错误类型说明
    switch (error) {
        case QSerialPort::DeviceNotFoundError:
            LOG_ERROR("Device not found error - port may be disconnected\n");
            break;
        case QSerialPort::PermissionError:
            LOG_ERROR("Permission error - port may be in use by another application\n");
            break;
        case QSerialPort::OpenError:
            LOG_ERROR("Open error - failed to open the port\n");
            break;
        case QSerialPort::WriteError:
            LOG_ERROR("Write error - failed to write data\n");
            break;
        case QSerialPort::ReadError:
            LOG_ERROR("Read error - failed to read data\n");
            break;
        case QSerialPort::ResourceError:
            LOG_ERROR("Resource error - device disappeared or became unavailable\n");
            break;
        case QSerialPort::UnsupportedOperationError:
            LOG_ERROR("Unsupported operation error\n");
            break;
        case QSerialPort::TimeoutError:
            LOG_ERROR("Timeout error\n");
            break;
        default:
            LOG_ERROR("Unknown error type: %d\n", (int)error);
            break;
    }
    
    // 设置错误状态
    m_serialStatus = STATUS_ERROR;
    
    // 根据错误类型处理
    switch (error) {
        case QSerialPort::ResourceError:
        case QSerialPort::DeviceNotFoundError:
        case QSerialPort::PermissionError:
            LOG_ERROR("Critical error - notifying connection callback\n");
            // 连接断开错误
            if (m_connectionCallback) {
                m_connectionCallback(false);
            }
            break;
        default:
            LOG_WARNING("Non-critical error - continuing operation\n");
            break;
    }
    
    LOG_ERROR("=== END SERIAL ERROR ===\n");
}

void SerialProtocol::ParseProtocolData(const QString& data)
{
    LOG_DEBUG("Parsing protocol data: %s\n", data.toStdString().c_str());
    
    // 移除协议开始和结束标识
    QString cleanData = data;
    cleanData.remove(PROTOCOL_START);
    cleanData.remove(PROTOCOL_END);
    
    // 按分隔符分割数据
    QStringList parts = cleanData.split(SEPARATOR);
    
    // 移除空的部分
    QStringList validParts;
    for (const QString& part : parts) {
        if (!part.isEmpty()) {
            validParts.append(part);
        }
    }
    
    
    if (validParts.isEmpty()) {
        LOG_WARNING("No valid parts found in protocol data\n");
        return;
    }
    
    QString command = validParts[0];
    LOG_DEBUG("Extracted command: '%s'\n", command.toStdString().c_str());
    
    if (command == CMD_START) {
        // 处理开始命令：$,START,# 或 $,START,cameraId,#
        LOG_INFO("Received START command\n");
        
        // 默认相机ID为1，如果有更多参数可以解析
        int cameraId = 1;
        if (validParts.size() > 1) {
            bool ok;
            cameraId = validParts[1].toInt(&ok);
            if (!ok) {
                cameraId = 1;
            }
        }
        
        // 触发工作信号回调
        if (m_workSignalCallback) {
            bool result = m_workSignalCallback(true, cameraId);
            LOG_INFO("Work signal callback result: %s, camera ID: %d\n", result ? "success" : "failed", cameraId);
        }
        
        m_serialStatus = STATUS_WORKING;
    } else {
        LOG_WARNING("Unknown command received: %s\n", command.toStdString().c_str());
    }
}

int SerialProtocol::SendData(const std::string& data)
{
    if (!m_pSerialPort || !m_pSerialPort->isOpen()) {
        LOG_ERROR("Serial port is not open, cannot send data\n");
        return -1;
    }
    
    // 检查串口是否可写
    if (!m_pSerialPort->isWritable()) {
        LOG_ERROR("Serial port is not writable\n");
        return -1;
    }
    
    // 检查待写入的字节数
    qint64 bytesToWrite = m_pSerialPort->bytesToWrite();
    LOG_INFO("Bytes pending write before send: %lld\n", bytesToWrite);
    
    QByteArray dataToSend = data.c_str();
    qint64 bytesWritten = m_pSerialPort->write(dataToSend);
    
    LOG_INFO("Write operation result: %lld bytes (expected: %d)\n", bytesWritten, dataToSend.size());
    
    if (bytesWritten == -1) {
        LOG_ERROR("Failed to write data to serial port: %s\n",  m_pSerialPort->errorString().toStdString().c_str());
        m_serialStatus = STATUS_ERROR;
        return -1;
    }
    
    if (bytesWritten != dataToSend.size()) {
        LOG_WARNING("PARTIAL WRITE: %lld of %d bytes written\n", bytesWritten, dataToSend.size());
    }

    // 立即刷新缓冲区
    m_pSerialPort->flush();
    
    
    LOG_DEBUG("Sent %lld bytes to serial port\n", bytesWritten);
    return SUCCESS;
}

std::string SerialProtocol::BuildDataProtocol(const MultiTargetData& multiTargetData, uint16_t cameraId)
{
    std::stringstream ss;
    
    // 协议格式：$,DATA,NUM,posX,posY,posZ,posC,posX,posY,posZ,posC...,#
    ss << PROTOCOL_START.toStdString() << SEPARATOR.toStdString()
       << CMD_DATA.toStdString() << SEPARATOR.toStdString()
       << multiTargetData.count;
    
    // 添加每个目标的坐标数据
    for (const auto& target : multiTargetData.targets) {
        ss << SEPARATOR.toStdString()
           << std::fixed << std::setprecision(2) << target.x << SEPARATOR.toStdString()
           << std::fixed << std::setprecision(2) << target.y << SEPARATOR.toStdString()
           << std::fixed << std::setprecision(2) << target.z << SEPARATOR.toStdString()
           << std::fixed << std::setprecision(2) << target.rz;
    }
    
    ss << SEPARATOR.toStdString() << PROTOCOL_END.toStdString();
    
    return ss.str();
}

// 线程方法实现
void SerialProtocol::StartReceiveThread()
{
    if (m_threadRunning.load()) {
        LOG_WARNING("Receive thread is already running\n");
        return;
    }
    
    LOG_INFO("Starting serial receive thread\n");
    m_threadRunning.store(true);
    
    // 启动接收线程
    m_receiveThread = std::thread(&SerialProtocol::SerialReceiveThreadFunction, this);
}

void SerialProtocol::StopReceiveThread()
{
    if (!m_threadRunning.load()) {
        LOG_DEBUG("Receive thread is not running\n");
        return;
    }
    
    LOG_INFO("Stopping serial receive thread\n");
    m_threadRunning.store(false);
    
    // 等待线程结束
    if (m_receiveThread.joinable()) {
        m_receiveThread.join();
        LOG_INFO("Serial receive thread stopped\n");
    }
}

void SerialProtocol::SerialReceiveThreadFunction()
{
    LOG_INFO("Serial receive thread started\n");
    
    while (m_threadRunning.load()) {
        // 检查串口是否打开
        if (!m_pSerialPort || !m_pSerialPort->isOpen()) {
            // 串口未打开，短暂休眠后继续检查
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            continue;
        }
        
        try {
            // 使用waitForReadyRead进行阻塞式等待，超时时间100ms
            if (m_pSerialPort->waitForReadyRead(100)) {
                // 有数据可读
                qint64 bytesAvailable = m_pSerialPort->bytesAvailable();
                if (bytesAvailable > 0) {
                    LOG_INFO("[Thread] Bytes available to read: %lld\n", bytesAvailable);
                    
                    // 读取所有可用数据
                    QByteArray data = m_pSerialPort->readAll();
                    if (!data.isEmpty()) {
                        QString receivedData = QString::fromUtf8(data);
                        
                        LOG_INFO("[Thread] Actually read %d bytes from serial port\n", data.size());
                        LOG_INFO("[Thread] Received [%d bytes]: \"%s\"\n", data.size(), receivedData.toStdString().c_str());
                        
                        // 使用互斥锁保护缓冲区
                        {
                            std::lock_guard<std::mutex> lock(m_bufferMutex);
                            m_receiveBuffer += receivedData;
                        }
                        
                        // 处理接收到的数据
                        ProcessReceivedData();
                    }
                }
            }
        } catch (const std::exception& e) {
            LOG_ERROR("[Thread] Exception in receive thread: %s\n", e.what());
        } catch (...) {
            LOG_ERROR("[Thread] Unknown exception in receive thread\n");
        }
    }
    
    LOG_INFO("Serial receive thread finished\n");
}

void SerialProtocol::ProcessReceivedData()
{
    std::lock_guard<std::mutex> lock(m_bufferMutex);
    
    // 查找完整的协议帧
    int startIndex = -1;
    int endIndex = -1;
    
    while ((startIndex = m_receiveBuffer.indexOf(PROTOCOL_START)) != -1) {
        endIndex = m_receiveBuffer.indexOf(PROTOCOL_END, startIndex);
        if (endIndex != -1) {
            // 提取完整的协议帧
            QString protocolFrame = m_receiveBuffer.mid(startIndex, endIndex - startIndex + 1);
            
            LOG_INFO("[Thread] Found complete protocol frame: %s\n", protocolFrame.toStdString().c_str());
            
            // 解析协议数据
            ParseProtocolData(protocolFrame);
            
            // 移除已处理的数据
            m_receiveBuffer.remove(0, endIndex + 1);
        } else {
            // 没有找到结束标识，等待更多数据
            break;
        }
    }
    
    // 如果缓冲区太大，清空一部分（防止内存溢出）
    if (m_receiveBuffer.length() > 1024) {
        LOG_WARNING("[Thread] Receive buffer too large (%d bytes), clearing\n", m_receiveBuffer.length());
        m_receiveBuffer.clear();
    }
}