#include "RobotProtocolSimplified.h"
#include "VrLog.h"
#include "VrError.h"
#include <cstring>

RobotProtocolSimplified::RobotProtocolSimplified()
    : m_pModbusServer(nullptr)
    , m_bServerRunning(false)
    , m_nPort(502)
    , m_connectionStatus(STATUS_DISCONNECTED)
{
}

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

int RobotProtocolSimplified::Initialize(uint16_t port)
{
    if (m_bServerRunning) {
        LOG_WARNING("Simplified protocol server is already running\n");
        return SUCCESS;
    }

    m_nPort = port;

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

    // 设置ModbusTCP回调函数
    if (m_pModbusServer) {
        // 设置写寄存器回调
        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_connectionStatus = STATUS_CONNECTED;

    // 初始化寄存器为0（无报警状态）
    ClearAlarmData();

    LOG_INFO("ModbusTCP simplified protocol service initialization completed on port %d\n", m_nPort);
    return SUCCESS;
}

void RobotProtocolSimplified::Deinitialize()
{
    LOG_DEBUG("Stop ModbusTCP simplified protocol service\n");

    // 停止ModbusTCP服务器
    StopModbusTCPServer();

    // 重置状态
    m_connectionStatus = STATUS_DISCONNECTED;
    m_bServerRunning = false;

    LOG_INFO("ModbusTCP simplified protocol service stopped\n");
}

int RobotProtocolSimplified::SetAlarmData(const TearingAlarmData& alarmData)
{
    if (!m_pModbusServer) {
        LOG_ERROR("ModbusTCP server not initialized\n");
        return ERR_CODE(DEV_NO_OPEN);
    }

    // 准备寄存器数据
    std::vector<uint16_t> data(TOTAL_REGISTERS, 0);

    // 地址0: 复位命令（保持不变，不在这里修改）
    // data[RESET_CMD_ADDR] = 0;

    // 地址1: 报警标志
    data[ALARM_FLAG_ADDR] = alarmData.alarmFlag;

    // 地址2: 最大长度
    data[MAX_LENGTH_ADDR] = alarmData.maxLength;

    // 地址3: 最大宽度
    data[MAX_WIDTH_ADDR] = alarmData.maxWidth;

    // 地址4-5: 最大撕裂ID (UInt32，大端字节序)
    // 高16位存储在地址4（40005）
    data[MAX_ID_ADDR] = (alarmData.maxId >> 16) & 0xFFFF;
    // 低16位存储在地址5（40006）
    data[MAX_ID_ADDR + 1] = alarmData.maxId & 0xFFFF;

    // 更新Modbus寄存器（从地址0开始，更新所有寄存器）
    m_pModbusServer->updateHoldingRegisters(RESET_CMD_ADDR, data);

    LOG_DEBUG("Alarm data updated: flag=%d, length=%d, width=%d, id=%u\n",
              alarmData.alarmFlag, alarmData.maxLength, alarmData.maxWidth, alarmData.maxId);

    return SUCCESS;
}

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

    // 清空所有寄存器
    std::vector<uint16_t> clearData(TOTAL_REGISTERS, 0);
    m_pModbusServer->updateHoldingRegisters(RESET_CMD_ADDR, clearData);

    LOG_INFO("Alarm data cleared (all registers reset to 0)\n");
    return SUCCESS;
}

void RobotProtocolSimplified::SetResetCallback(const ResetCallback& callback)
{
    m_resetCallback = callback;
}

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

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

void RobotProtocolSimplified::StopModbusTCPServer()
{
    LOG_DEBUG("Stop ModbusTCP simplified protocol server\n");

    if (m_pModbusServer) {
        // 释放资源
        delete m_pModbusServer;
        m_pModbusServer = nullptr;
    }

    m_bServerRunning = false;
    LOG_INFO("ModbusTCP simplified protocol server stopped\n");
}

IYModbusTCPServer::ErrorCode RobotProtocolSimplified::OnWriteRegisters(uint8_t unitId, uint16_t startAddress, uint16_t quantity, const uint16_t* values)
{
    // 只允许写入地址0（复位命令寄存器40001）
    if (startAddress != RESET_CMD_ADDR) {
        LOG_WARNING("Attempt to write to read-only address: %d (only address 0 is writable)\n", startAddress);
        return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_ADDRESS;
    }

    // 只允许写入单个寄存器
    if (quantity != 1) {
        LOG_WARNING("Invalid quantity for reset command 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 resetValue = values[0];

    // 任何非零值都触发复位
    if (resetValue != 0) {
        LOG_INFO("Received reset command via ModbusTCP (value: %d)\n", resetValue);

        // 调用复位回调
        if (m_resetCallback) {
            m_resetCallback();
            LOG_DEBUG("Reset callback executed\n");
        } else {
            LOG_WARNING("Reset callback not set\n");
        }

        // 执行复位操作：清空报警数据
        ClearAlarmData();
    }

    return IYModbusTCPServer::ErrorCode::SUCCESS;
}

void RobotProtocolSimplified::OnModbusTCPConnectionChanged(bool connected)
{
    LOG_INFO("ModbusTCP simplified protocol connection status changed: %s\n", connected ? "connected" : "disconnected");

    // 更新连接状态
    m_connectionStatus = connected ? STATUS_CONNECTED : STATUS_DISCONNECTED;

    // 调用连接状态回调
    if (m_connectionCallback) {
        m_connectionCallback(m_connectionStatus);
    }
}