GrabBag/App/BeltTearing/BeltTearingServer/RobotProtocol.cpp

#include "RobotProtocol.h"
#include "VrLog.h"
#include "VrError.h"
#include "IYModbusTCPServer.h"
#include <cstring>
#include <algorithm>

RobotProtocol::RobotProtocol()
    : m_pModbusServer(nullptr)
    , m_bServerRunning(false)
    , m_nPort(502)
    , m_robotStatus(STATUS_DISCONNECTED)
{
}

RobotProtocol::~RobotProtocol()
{
    Deinitialize();
}

int RobotProtocol::Initialize(uint16_t port)
{
    if (m_bServerRunning) {
        LOG_WARNING("Server is already running\n");
        return SUCCESS;
    }
    
    m_nPort = port;

    // 创建ModbusTCP服务器实例
    bool bRet = IYModbusTCPServer::CreateInstance(&m_pModbusServer);
    LOG_DEBUG("Create ModbusTCP server %s \n", bRet ? "success" : "failed");
    m_bServerRunning = bRet;

    // 设置ModbusTCP回调函数
    if (m_pModbusServer) {
        // 设置写线圈回调
        m_pModbusServer->setWriteCoilsCallback([this](uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint8_t* values) {
            return this->OnWriteCoils(unitId, startAddress, quantity, values);
        });
        
        // 设置写寄存器回调
        m_pModbusServer->setWriteRegistersCallback([this](uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint16_t* values) {
            return this->OnWriteRegisters(unitId, startAddress, quantity, values);
        });
        
        // 设置连接状态回调
        m_pModbusServer->setConnectionStatusCallback([this](bool connected) {
            this->OnModbusTCPConnectionChanged(connected);
        });
    }

    int nRet = m_pModbusServer->start(m_nPort);
    ERR_CODE_RETURN(nRet);

    // 设置初始状态
    m_robotStatus = STATUS_CONNECTED;
    
    // 设置初始工作状态为空闲
    SetWorkStatus(WORK_STATUS_IDLE);

    LOG_INFO("ModbusTCP service initialization completed\n");
    return SUCCESS;
}

void RobotProtocol::Deinitialize()
{
    LOG_DEBUG("Stop ModbusTCP service\n");

    // 停止ModbusTCP服务器
    StopModbusTCPServer();
    
    // 重置状态
    m_robotStatus = STATUS_DISCONNECTED;
    m_bServerRunning = false;
    
    LOG_INFO("ModbusTCP service stopped\n");
}

int RobotProtocol::SetMultiTargetData(const MultiTargetData& multiTargetData)
{
    // 数据格式: count + 每个目标的{id,status,width,depth}数据（每个数据占2个寄存器）
    std::vector<uint16_t> data;
    // 限制最大目标数量
    size_t actualCount = multiTargetData.targets.size() > 5 ? 5 : multiTargetData.targets.size();
    data.resize(actualCount * 16);
    memset(data.data(), 0, data.size() * sizeof(uint16_t));
    
    // 定义一个辅助函数来转换uint32_t到ModbusTCP寄存器顺序大端格式
    auto uint32ToBigEndian = [&](uint16_t addr, uint32_t value) {
        // ModbusTCP寄存器顺序大端：高16位寄存器在前，低16位寄存器在后
        data[addr] = (value >> 16) & 0xFFFF;  // 高16位寄存器
        data[addr+1] = value & 0xFFFF;          // 低16位寄存器
    };
    
    // 定义一个辅助函数来转换float到ModbusTCP寄存器顺序大端格式
    auto floatToBigEndian = [&](uint16_t addr, float value) {
        uint32_t intValue;
        memcpy(&intValue, &value, sizeof(float));
        // ModbusTCP寄存器顺序大端：高16位寄存器在前，低16位寄存器在后
        data[addr] = (intValue >> 16) & 0xFFFF;  // 高16位寄存器
        data[addr+1] = intValue & 0xFFFF;          // 低16位寄存器
    };
    
    // 定义一个辅助函数来转换uint16_t到ModbusTCP寄存器
    auto uint16ToBigEndian = [&](uint16_t addr, uint16_t value) {
        data[addr] = value;  // uint16_t直接存储
    };
        
    // 从第二个寄存器开始存储目标数据 count [{id,status,width,depth}, {id,status,width,depth}]
    for (size_t i = 0; i < actualCount; i++) {
        uint16_t addr = i * RESULT_OFFSET_ADDR;
        const TargetResult& target = multiTargetData.targets[i];
        
        // 按照 id, status, width, depth 的顺序存储每个目标的数据
        uint32ToBigEndian(addr, target.id);         // ID (2个寄存器)
        uint16ToBigEndian(addr+2, target.status);   // Status (1个寄存器)
        floatToBigEndian(addr+3, target.width);     // Width (2个寄存器)
        floatToBigEndian(addr+5, target.depth);     // Depth (2个寄存器)
    }
    
    m_pModbusServer->updateHoldingRegisters(DETECT_RESULT_ADDR, data);
    
    return SUCCESS;
}

RobotProtocol::RobotStatus RobotProtocol::GetDetectionStatus() const
{
    return m_robotStatus;
}

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

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

void RobotProtocol::SetSystemControlCallback(const std::function<void(uint16_t)>& callback)
{
    m_systemControlCallback = callback;
}

bool RobotProtocol::IsRunning() const
{
    return m_bServerRunning;
}

int RobotProtocol::SetWorkStatus(uint16_t status)
{
    LOG_DEBUG("Set work status to: %d\n", status);

    if (!m_pModbusServer) {
        LOG_ERROR("ModbusTCP server not initialized\n");
        return ERR_CODE(DEV_NO_OPEN);
    }

    // 更新STATUS_ADDR地址的寄存器值
    std::vector<uint16_t> statusData;
    statusData.push_back(status);

    m_pModbusServer->updateHoldingRegisters(STATUS_ADDR, statusData);

    const char* statusStr = "";
    switch(status) {
        case WORK_STATUS_IDLE:          statusStr = "空闲"; break;
        case WORK_STATUS_WORKING:       statusStr = "工作中"; break;
        case WORK_STATUS_BUSY:          statusStr = "忙碌"; break;
        default:                        statusStr = "未知状态"; break;
    }

    LOG_INFO("Work status updated to: %d (%s)\n", status, statusStr);
    return SUCCESS;
}

int RobotProtocol::ClearDetectionData()
{
    LOG_INFO("Clearing detection result data in Modbus registers\n");

    if (!m_pModbusServer) {
        LOG_ERROR("ModbusTCP server not initialized\n");
        return ERR_CODE(DEV_NO_OPEN);
    }

    // 清空检测结果数据区域（地址1-80，共5个撕裂信息，每个16个寄存器）
    // 总共需要清空 5 * 16 = 80 个寄存器
    std::vector<uint16_t> clearData(80, 0);  // 创建80个值为0的寄存器

    m_pModbusServer->updateHoldingRegisters(DETECT_RESULT_ADDR, clearData);

    LOG_INFO("Detection result data cleared (registers 1-80)\n");
    return SUCCESS;
}

void RobotProtocol::StopModbusTCPServer()
{
    LOG_DEBUG("Stop ModbusTCP server\n");
    
    if (m_pModbusServer) {
        // 释放资源
        delete m_pModbusServer;
        m_pModbusServer = nullptr;
    }
    
    m_bServerRunning = false;
    LOG_INFO("ModbusTCP server stopped\n");
}

IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteCoils(uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint8_t* values)
{
    return IYModbusTCPServer::ErrorCode::ILLEGAL_FUNCTION;
}

IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteRegisters(uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint16_t* values)
{
    // 只允许写入地址0（系统状态寄存器）
    if (startAddress != STATUS_ADDR) {
        LOG_WARNING("Attempt to write to read-only address: %d\n", startAddress);
        return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_ADDRESS;
    }

    // 只允许写入单个寄存器
    if (quantity != 1) {
        LOG_WARNING("Invalid quantity for status register write: %d\n", quantity);
        return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_VALUE;
    }

    if (!values) {
        LOG_ERROR("Null values pointer in write registers\n");
        return IYModbusTCPServer::ErrorCode::SERVER_FAILURE;
    }

    uint16_t command = values[0];

    // 验证命令值是否有效（0-停止, 1-开始, 2-复位）
    if (command > 2) {
        LOG_WARNING("Invalid system control command: %d\n", command);
        return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_VALUE;
    }

    LOG_INFO("Received system control command via ModbusTCP: %d\n", command);

    // 调用系统控制回调
    if (m_systemControlCallback) {
        m_systemControlCallback(command);
        LOG_DEBUG("System control callback executed for command: %d\n", command);
    } else {
        LOG_WARNING("System control callback not set\n");
    }

    return IYModbusTCPServer::ErrorCode::SUCCESS;
}

void RobotProtocol::OnModbusTCPConnectionChanged(bool connected)
{

}