#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("Server is already running\n");
        return SUCCESS;
    }
    
    m_nPort = port;

    // 创建ModbusTCP服务器实例
    bool bRet = IYModbusTCPServer::CreateInstance(&m_pModbusServer);
    LOG_ERRO("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();
    ERR_CODE_RETURN(nRet);

    // 设置初始状态
    m_robotStatus = STATUS_CONNECTED;

    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::SetRobotCoordinate(const RobotCoordinate& coordinate)
{
    LOG_DEBUG("Set robot arm coordinates\n");
    LOG_DEBUG("Coordinates: X=%.2f, Y=%.2f, Z=%.2f\n", coordinate.x, coordinate.y, coordinate.z);
    LOG_DEBUG("Rotation: 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("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)
{
    LOG_DEBUG("Write coils - UnitID:%d, StartAddress:%d, Quantity:%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("Received work signal: %s\n", (workSignal ? "start work" : "stop work"));
        
        // 触发工作信号回调
        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("Write registers - UnitID:%d, StartAddress:%d, Quantity:%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("Read holding registers - UnitID:%d, StartAddress:%d, Quantity:%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;
}

void RobotProtocol::OnModbusTCPConnectionChanged(bool connected)
{
    if (connected) {
        LOG_INFO("ModbusTCP connection established\n");
        m_robotStatus = STATUS_CONNECTED;
    } else {
        LOG_INFO("ModbusTCP connection lost\n");
        m_robotStatus = STATUS_DISCONNECTED;
    }
    
    // 触发上层连接状态回调
    if (m_connectionCallback) {
        m_connectionCallback(connected);
    }
}