#include "RobotProtocol.h"
#include "VrSimpleLog.h"
#include "VrError.h"
#include <cstring>

RobotProtocol::RobotProtocol()
    : m_pModbusServer(nullptr)
    , m_bServerRunning(false)
    , m_nPort(502)
    , m_robotStatus(STATUS_DISCONNECTED)
{
    // 初始化坐标
    memset(&m_robotCoordinate, 0, sizeof(m_robotCoordinate));
}

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

int RobotProtocol::Initialize(uint16_t port)
{
    if (m_bServerRunning) {
        LOG_WARNING("RobotProtocol::Initialize - 服务已经在运行中\n");
        return SUCCESS;
    }
    
    m_nPort = port;

    // 创建ModbusTCP服务器实例
    bool bRet = IYModbusTCPServer::CreateInstance(&m_pModbusServer);
    LOG_ERRO("RobotProtocol::%s - 创建ModbusTCP服务器%s \n", __func__, bRet ? "失败" : "成功");
    m_bServerRunning = bRet;
    ERR_CODE_RETURN(NET_ERR_CREAT_INIT);
    
    // 设置初始状态
    m_robotStatus = STATUS_CONNECTED;

    LOG_INFO("RobotProtocol::Initialize - ModbusTCP服务初始化完成\n");
    return 0;
}

void RobotProtocol::Deinitialize()
{
    LOG_DEBUG("RobotProtocol::Deinitialize - 停止ModbusTCP服务\n");

    // 停止ModbusTCP服务器
    StopModbusTCPServer();
    
    // 重置状态
    m_robotStatus = STATUS_DISCONNECTED;
    m_bServerRunning = false;
    
    LOG_INFO("RobotProtocol::Deinitialize - ModbusTCP服务已停止\n");
}

int RobotProtocol::SetRobotCoordinate(const RobotCoordinate& coordinate)
{
    LOG_DEBUG("RobotProtocol::SetRobotCoordinate - 设置机械臂坐标\n");
    LOG_DEBUG("坐标: X=%.2f, Y=%.2f, Z=%.2f\n", coordinate.x, coordinate.y, coordinate.z);
    LOG_DEBUG("旋转: RX=%.2f, RY=%.2f, RZ=%.2f\n", coordinate.rx, coordinate.ry, coordinate.rz);
    
    m_robotCoordinate = coordinate;
    
    return 0;
}

int RobotProtocol::GetRobotCoordinate(RobotCoordinate& coordinate)
{
    coordinate = m_robotCoordinate;
    return 0;
}

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;
}

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

void RobotProtocol::StopModbusTCPServer()
{
    LOG_DEBUG("RobotProtocol::StopModbusTCPServer - 停止ModbusTCP服务器\n");
    
    if (m_pModbusServer) {
        // 释放资源
        delete m_pModbusServer;
        m_pModbusServer = nullptr;
    }
    
    m_bServerRunning = false;
    LOG_INFO("RobotProtocol::StopModbusTCPServer - ModbusTCP服务器已停止\n");
}

IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteCoils(uint8_t unitId, uint16_t startAddress, 
                                                        uint16_t quantity, const uint8_t* values)
{
    LOG_DEBUG("RobotProtocol::OnWriteCoils - 单元ID:%d, 起始地址:%d, 数量:%d\n", 
              unitId, startAddress, quantity);
    
    // 处理工作信号线圈
    if (startAddress <= WORK_SIGNAL_COIL && startAddress + quantity > WORK_SIGNAL_COIL) {
        uint16_t coilIndex = WORK_SIGNAL_COIL - startAddress;
        bool workSignal = (values[coilIndex / 8] >> (coilIndex % 8)) & 0x01;
        
        LOG_INFO("RobotProtocol::OnWriteCoils - 收到工作信号:%s\n", 
                 (workSignal ? "开始工作" : "停止工作"));
        
        // 触发工作信号回调
        if (m_workSignalCallback) {
            m_workSignalCallback(workSignal);
        }
    }
    
    return IYModbusTCPServer::ErrorCode::SUCCESS;
}

IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteRegisters(uint8_t unitId, uint16_t startAddress, 
                                                            uint16_t quantity, const uint16_t* values)
{
    LOG_DEBUG("RobotProtocol::OnWriteRegisters - 单元ID:%d, 起始地址:%d, 数量:%d\n", 
              unitId, startAddress, quantity);
    
    // 处理坐标寄存器更新
    if (startAddress >= COORD_X_ADDR && startAddress < STATUS_ADDR) {
        RobotCoordinate newCoordinate = m_robotCoordinate;
        
        for (uint16_t i = 0; i < quantity; i++) {
            uint16_t addr = startAddress + i;
            uint16_t value = values[i];
            
            // 根据地址更新相应的坐标分量
            if (addr >= COORD_X_ADDR && addr < COORD_X_ADDR + 2) {
                memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.x) + (addr - COORD_X_ADDR), &value, sizeof(uint16_t));
            } else if (addr >= COORD_Y_ADDR && addr < COORD_Y_ADDR + 2) {
                memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.y) + (addr - COORD_Y_ADDR), &value, sizeof(uint16_t));
            } else if (addr >= COORD_Z_ADDR && addr < COORD_Z_ADDR + 2) {
                memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.z) + (addr - COORD_Z_ADDR), &value, sizeof(uint16_t));
            } else if (addr >= COORD_RX_ADDR && addr < COORD_RX_ADDR + 2) {
                memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.rx) + (addr - COORD_RX_ADDR), &value, sizeof(uint16_t));
            } else if (addr >= COORD_RY_ADDR && addr < COORD_RY_ADDR + 2) {
                memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.ry) + (addr - COORD_RY_ADDR), &value, sizeof(uint16_t));
            } else if (addr >= COORD_RZ_ADDR && addr < COORD_RZ_ADDR + 2) {
                memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.rz) + (addr - COORD_RZ_ADDR), &value, sizeof(uint16_t));
            }
        }
        
        // 更新坐标
        SetRobotCoordinate(newCoordinate);
    }
    
    return IYModbusTCPServer::ErrorCode::SUCCESS;
}

IYModbusTCPServer::ErrorCode RobotProtocol::OnReadHoldingRegisters(uint8_t unitId, uint16_t startAddress, 
                                                                  uint16_t quantity, uint16_t* values)
{
    LOG_DEBUG("RobotProtocol::OnReadHoldingRegisters - 单元ID:%d, 起始地址:%d, 数量:%d\n", 
              unitId, startAddress, quantity);
    
    for (uint16_t i = 0; i < quantity; i++) {
        uint16_t addr = startAddress + i;
        
        if (addr >= COORD_X_ADDR && addr < COORD_X_ADDR + 2) {
            // X坐标
            memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.x) + (addr - COORD_X_ADDR), sizeof(uint16_t));
        } else if (addr >= COORD_Y_ADDR && addr < COORD_Y_ADDR + 2) {
            // Y坐标
            memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.y) + (addr - COORD_Y_ADDR), sizeof(uint16_t));
        } else if (addr >= COORD_Z_ADDR && addr < COORD_Z_ADDR + 2) {
            // Z坐标
            memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.z) + (addr - COORD_Z_ADDR), sizeof(uint16_t));
        } else if (addr >= COORD_RX_ADDR && addr < COORD_RX_ADDR + 2) {
            // RX坐标
            memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.rx) + (addr - COORD_RX_ADDR), sizeof(uint16_t));
        } else if (addr >= COORD_RY_ADDR && addr < COORD_RY_ADDR + 2) {
            // RY坐标
            memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.ry) + (addr - COORD_RY_ADDR), sizeof(uint16_t));
        } else if (addr >= COORD_RZ_ADDR && addr < COORD_RZ_ADDR + 2) {
            // RZ坐标
            memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.rz) + (addr - COORD_RZ_ADDR), sizeof(uint16_t));
        } else if (addr == STATUS_ADDR) {
            // 状态寄存器
            values[i] = static_cast<uint16_t>(m_robotStatus);
        } else {
            // 未定义的地址，返回0
            values[i] = 0;
        }
    }
    
    return IYModbusTCPServer::ErrorCode::SUCCESS;
}