#include "RobotProtocol.h"
#include "VrLog.h"
#include "VrError.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, uint16_t cameraId)
{
    LOG_DEBUG("Set multi-target data, count: %d, camera ID: %d\n", multiTargetData.count, cameraId);

    // 验证相机ID有效性（从1开始编号：1，2，...）
    if (cameraId < 1 || cameraId > 2) {
        LOG_ERROR("Invalid camera ID: %d, valid range is 1-2\n", cameraId);
        return -1;
    }

    // 数据格式: count + 每个目标的{x,y,z,rz}坐标（每个坐标占2个寄存器）
    std::vector<uint16_t> data;
    // 限制最大目标数量
    uint16_t actualCount = multiTargetData.count > 5 ? 5 : multiTargetData.count;
    data.resize(actualCount * 8 + 1);
    
    // 根据相机ID确定起始地址（cameraId: 1->COORD_ONE, 2->COORD_TWO）
    uint16_t startAddr = (cameraId == 1) ? COORD_ONE_START_ADDR : COORD_TWO_START_ADDR;
    
    // 设置目标数量
    data[0] = actualCount;

    // 从第二个寄存器开始存储坐标数据 count [{x,y,z,rz}, {x,y,z,rz}]
    uint16_t dataIndex = 2;
    
    // 定义一个辅助函数来转换float到ModbusTCP寄存器顺序大端格式
    auto floatToBigEndian = [&](float value) {
        uint32_t intValue;
        memcpy(&intValue, &value, sizeof(float));
        // ModbusTCP寄存器顺序大端：高16位寄存器在前，低16位寄存器在后
        // 每个寄存器内部保持主机字节序
        data[dataIndex++] = (intValue >> 16) & 0xFFFF;  // 高16位寄存器
        data[dataIndex++] = intValue & 0xFFFF;          // 低16位寄存器
    };
    
    for (uint16_t i = 0; i < actualCount; i++) {
        const TargetPosition& target = multiTargetData.targets[i];
        
        // 按照 x, y, z, rz 的顺序存储每个目标的坐标
        floatToBigEndian(target.x);   // X坐标 (2个寄存器)
        floatToBigEndian(target.y);   // Y坐标 (2个寄存器)
        floatToBigEndian(target.z);   // Z坐标 (2个寄存器)
        floatToBigEndian(target.rz);  // RZ坐标 (2个寄存器)
    }
    
    m_pModbusServer->updateHoldingRegisters(startAddr, data);
    
    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;
}

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 -1;
    }
    
    // 更新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_CAMERA1_DONE:  statusStr = "相机1工作完成"; break;
        case WORK_STATUS_CAMERA2_DONE:  statusStr = "相机2工作完成"; break;
        case WORK_STATUS_BUSY:          statusStr = "忙碌"; break;
        default:                        statusStr = "未知状态"; break;
    }
    
    LOG_INFO("Work status updated to: %d (%s)\n", status, statusStr);
    return 0;
}

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_ADDR) {
        bool workSignal = (values[0] >> 0) & 0x01;
        
        LOG_INFO("Received work signal: %s (coil address: %d)\n", 
                 (workSignal ? "start work" : "stop work"), startAddress);
        
        // 触发工作信号回调（使用相机ID 1作为默认值）
        if (m_workSignalCallback) {
            m_workSignalCallback(workSignal, 1);  // 默认相机ID为1
        }
    } else {
        LOG_WARNING("Unknown coil address: %d\n", startAddress);
        return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_ADDRESS;
    }
    
    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(quantity > 1) return IYModbusTCPServer::ErrorCode::ILLEGAL_DATA_VALUE;

    if(nullptr == m_workSignalCallback) return IYModbusTCPServer::ErrorCode::GATEWAY_TARGET_FAILED;

    IYModbusTCPServer::ErrorCode eResult = IYModbusTCPServer::ErrorCode::SUCCESS;
    
    switch (startAddress) {
    case WORK_SIGNAL_ADDR:
        // 解析工作信号寄存器
        if(!m_workSignalCallback(true, (int)values[0])){
            eResult = IYModbusTCPServer::ErrorCode::SERVER_FAILURE;
        }
        break;
    default:
        eResult = IYModbusTCPServer::ErrorCode::ILLEGAL_FUNCTION;
        break;
    }
    
    return eResult;
}

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