#include "../Inc/ModbusTCPClient.h"
#include <iostream>
#include <cstring>
#include <cstdlib>

ModbusTCPClient::ModbusTCPClient(const std::string& serverIP, int serverPort)
    : m_modbusContext(nullptr)
    , m_serverIP(serverIP)
    , m_serverPort(serverPort)
    , m_slaveId(1)
    , m_timeoutMs(1000)
    , m_connectionState(DISCONNECTED)
{
    // 创建TCP上下文
    m_modbusContext = modbus_new_tcp(m_serverIP.c_str(), m_serverPort);
    if (!m_modbusContext) {
        SetLastError("Failed to create modbus TCP context");
        SetConnectionState(ERROR_STATE, "Failed to create modbus context");
    }
}

ModbusTCPClient::~ModbusTCPClient()
{
    Disconnect();
    if (m_modbusContext) {
        modbus_free(m_modbusContext);
        m_modbusContext = nullptr;
    }
}

bool ModbusTCPClient::SetConnectionParams(const std::string& serverIP, int serverPort)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (m_connectionState == CONNECTED) {
        SetLastError("Cannot change connection parameters while connected");
        return false;
    }
    
    m_serverIP = serverIP;
    m_serverPort = serverPort;
    
    // 重新创建modbus上下文
    if (m_modbusContext) {
        modbus_free(m_modbusContext);
    }
    
    m_modbusContext = modbus_new_tcp(m_serverIP.c_str(), m_serverPort);
    if (!m_modbusContext) {
        SetLastError("Failed to create modbus TCP context with new parameters");
        SetConnectionState(ERROR_STATE, "Failed to recreate modbus context");
        return false;
    }
    
    // 重新设置之前的参数
    modbus_set_slave(m_modbusContext, m_slaveId);
    
    uint32_t sec = m_timeoutMs / 1000;
    uint32_t usec = (m_timeoutMs % 1000) * 1000;
    modbus_set_response_timeout(m_modbusContext, sec, usec);
    
    return true;
}

bool ModbusTCPClient::SetSlaveId(int slaveId)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (slaveId < 1 || slaveId > 247) {
        SetLastError("Invalid slave ID. Must be between 1 and 247");
        return false;
    }
    
    m_slaveId = slaveId;
    
    if (m_modbusContext) {
        if (modbus_set_slave(m_modbusContext, m_slaveId) == -1) {
            SetLastError("Failed to set slave ID");
            return false;
        }
    }
    
    return true;
}

bool ModbusTCPClient::SetTimeout(int timeoutMs)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (timeoutMs <= 0) {
        SetLastError("Invalid timeout. Must be greater than 0");
        return false;
    }
    
    m_timeoutMs = timeoutMs;
    
    if (m_modbusContext) {
        uint32_t sec = timeoutMs / 1000;
        uint32_t usec = (timeoutMs % 1000) * 1000;
        
        if (modbus_set_response_timeout(m_modbusContext, sec, usec) == -1) {
            SetLastError("Failed to set response timeout");
            return false;
        }
    }
    
    return true;
}

void ModbusTCPClient::SetConnectionStateCallback(ConnectionStateCallback callback)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_stateCallback = callback;
}

ModbusTCPClient::Result ModbusTCPClient::Connect()
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsModbusContextValid()) {
        SetLastError("Invalid modbus context");
        return ERROR_CONNECTION;
    }
    
    if (m_connectionState == CONNECTED) {
        return SUCCESS;
    }
    
    SetConnectionState(CONNECTING, "Attempting to connect");
    
    // 设置参数
    modbus_set_slave(m_modbusContext, m_slaveId);
    
    uint32_t sec = m_timeoutMs / 1000;
    uint32_t usec = (m_timeoutMs % 1000) * 1000;
    modbus_set_response_timeout(m_modbusContext, sec, usec);
    
    // 尝试连接
    if (modbus_connect(m_modbusContext) == -1) {
        std::string errorMsg = "Failed to connect to server";
        SetLastError(errorMsg);
        SetConnectionState(ERROR_STATE, errorMsg);
        return ERROR_CONNECTION;
    }
    
    SetConnectionState(CONNECTED, "Successfully connected");
    return SUCCESS;
}

void ModbusTCPClient::Disconnect()
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (m_modbusContext && m_connectionState == CONNECTED) {
        modbus_close(m_modbusContext);
        SetConnectionState(DISCONNECTED, "Disconnected");
    }
}

ModbusTCPClient::ConnectionState ModbusTCPClient::GetConnectionState() const
{
    return m_connectionState.load();
}

bool ModbusTCPClient::IsConnected() const
{
    return m_connectionState.load() == CONNECTED;
}

ModbusTCPClient::Result ModbusTCPClient::ReadCoils(int startAddress, int quantity, std::vector<bool>& values)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    if (quantity <= 0 || quantity > 2000) {
        SetLastError("Invalid quantity. Must be between 1 and 2000");
        return ERROR_INVALID_PARAM;
    }
    
    std::vector<uint8_t> buffer(quantity);
    int result = modbus_read_bits(m_modbusContext, startAddress, quantity, buffer.data());
    
    if (result == -1) {
        SetLastError("Failed to read coils");
        return ConvertLibmodbusError();
    }
    
    values.clear();
    values.reserve(quantity);
    for (int i = 0; i < quantity; ++i) {
        values.push_back(buffer[i] != 0);
    }
    
    return SUCCESS;
}

ModbusTCPClient::Result ModbusTCPClient::ReadDiscreteInputs(int startAddress, int quantity, std::vector<bool>& values)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    if (quantity <= 0 || quantity > 2000) {
        SetLastError("Invalid quantity. Must be between 1 and 2000");
        return ERROR_INVALID_PARAM;
    }
    
    std::vector<uint8_t> buffer(quantity);
    int result = modbus_read_input_bits(m_modbusContext, startAddress, quantity, buffer.data());
    
    if (result == -1) {
        SetLastError("Failed to read discrete inputs: " + std::string(modbus_strerror(errno)));
        return ConvertLibmodbusError();
    }
    
    values.clear();
    values.reserve(quantity);
    for (int i = 0; i < quantity; ++i) {
        values.push_back(buffer[i] != 0);
    }
    
    return SUCCESS;
}

ModbusTCPClient::Result ModbusTCPClient::ReadHoldingRegisters(int startAddress, int quantity, std::vector<uint16_t>& values)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    if (quantity <= 0 || quantity > 125) {
        SetLastError("Invalid quantity. Must be between 1 and 125");
        return ERROR_INVALID_PARAM;
    }
    
    values.resize(quantity);
    int result = modbus_read_registers(m_modbusContext, startAddress, quantity, values.data());
    
    if (result == -1) {
        SetLastError("Failed to read holding registers: " + std::string(modbus_strerror(errno)));
        return ConvertLibmodbusError();
    }
    
    return SUCCESS;
}

ModbusTCPClient::Result ModbusTCPClient::ReadInputRegisters(int startAddress, int quantity, std::vector<uint16_t>& values)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    if (quantity <= 0 || quantity > 125) {
        SetLastError("Invalid quantity. Must be between 1 and 125");
        return ERROR_INVALID_PARAM;
    }
    
    values.resize(quantity);
    int result = modbus_read_input_registers(m_modbusContext, startAddress, quantity, values.data());
    
    if (result == -1) {
        SetLastError("Failed to read input registers: " + std::string(modbus_strerror(errno)));
        return ConvertLibmodbusError();
    }
    
    return SUCCESS;
}

ModbusTCPClient::Result ModbusTCPClient::WriteSingleCoil(int address, bool value)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    int result = modbus_write_bit(m_modbusContext, address, value ? TRUE : FALSE);
    
    if (result == -1) {
        SetLastError("Failed to write single coil: " + std::string(modbus_strerror(errno)));
        return ConvertLibmodbusError();
    }
    
    return SUCCESS;
}

ModbusTCPClient::Result ModbusTCPClient::WriteSingleRegister(int address, uint16_t value)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    int result = modbus_write_register(m_modbusContext, address, value);
    
    if (result == -1) {
        SetLastError("Failed to write single register: " + std::string(modbus_strerror(errno)));
        return ConvertLibmodbusError();
    }
    
    return SUCCESS;
}

ModbusTCPClient::Result ModbusTCPClient::WriteMultipleCoils(int startAddress, const std::vector<bool>& values)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    if (values.empty() || values.size() > 1968) {
        SetLastError("Invalid values size. Must be between 1 and 1968");
        return ERROR_INVALID_PARAM;
    }
    
    std::vector<uint8_t> buffer(values.size());
    for (size_t i = 0; i < values.size(); ++i) {
        buffer[i] = values[i] ? TRUE : FALSE;
    }
    
    int result = modbus_write_bits(m_modbusContext, startAddress, static_cast<int>(values.size()), buffer.data());
    
    if (result == -1) {
        SetLastError("Failed to write multiple coils: " + std::string(modbus_strerror(errno)));
        return ConvertLibmodbusError();
    }
    
    return SUCCESS;
}

ModbusTCPClient::Result ModbusTCPClient::WriteMultipleRegisters(int startAddress, const std::vector<uint16_t>& values)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    if (values.empty() || values.size() > 123) {
        SetLastError("Invalid values size. Must be between 1 and 123");
        return ERROR_INVALID_PARAM;
    }
    
    int result = modbus_write_registers(m_modbusContext, startAddress, static_cast<int>(values.size()), values.data());
    
    if (result == -1) {
        SetLastError("Failed to write multiple registers: " + std::string(modbus_strerror(errno)));
        return ConvertLibmodbusError();
    }
    
    return SUCCESS;
}

ModbusTCPClient::Result ModbusTCPClient::ReadWriteMultipleRegisters(int readStartAddress, int readQuantity,
                                                                    int writeStartAddress, const std::vector<uint16_t>& writeValues,
                                                                    std::vector<uint16_t>& readValues)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    
    if (!IsConnected()) {
        SetLastError("Not connected to server");
        return ERROR_CONNECTION;
    }
    
    if (readQuantity <= 0 || readQuantity > 125) {
        SetLastError("Invalid read quantity. Must be between 1 and 125");
        return ERROR_INVALID_PARAM;
    }
    
    if (writeValues.empty() || writeValues.size() > 121) {
        SetLastError("Invalid write values size. Must be between 1 and 121");
        return ERROR_INVALID_PARAM;
    }
    
    readValues.resize(readQuantity);
    
    int result = modbus_write_and_read_registers(m_modbusContext, 
                                                writeStartAddress, static_cast<int>(writeValues.size()), writeValues.data(),
                                                readStartAddress, readQuantity, readValues.data());
    
    if (result == -1) {
        SetLastError("Failed to read/write multiple registers: " + std::string(modbus_strerror(errno)));
        return ConvertLibmodbusError();
    }
    
    return SUCCESS;
}

// 静态辅助函数实现
uint32_t ModbusTCPClient::RegistersToUInt32(uint16_t high, uint16_t low)
{
    return (static_cast<uint32_t>(high) << 16) | low;
}

void ModbusTCPClient::UInt32ToRegisters(uint32_t value, uint16_t& high, uint16_t& low)
{
    high = static_cast<uint16_t>((value >> 16) & 0xFFFF);
    low = static_cast<uint16_t>(value & 0xFFFF);
}

float ModbusTCPClient::RegistersToFloat(uint16_t high, uint16_t low)
{
    union {
        uint32_t i;
        float f;
    } converter;
    
    converter.i = RegistersToUInt32(high, low);
    return converter.f;
}

void ModbusTCPClient::FloatToRegisters(float value, uint16_t& high, uint16_t& low)
{
    union {
        uint32_t i;
        float f;
    } converter;
    
    converter.f = value;
    UInt32ToRegisters(converter.i, high, low);
}

std::string ModbusTCPClient::GetLastError() const
{
    std::lock_guard<std::mutex> lock(m_mutex);
    return m_lastError;
}

std::string ModbusTCPClient::ResultToString(Result result)
{
    switch (result) {
        case SUCCESS: return "Success";
        case ERROR_CONNECTION: return "Connection Error";
        case ERROR_INVALID_PARAM: return "Invalid Parameter";
        case ERROR_TIMEOUT: return "Timeout Error";
        case ERROR_DEVICE: return "Device Error";
        case ERROR_PROTOCOL: return "Protocol Error";
        case ERROR_UNKNOWN: return "Unknown Error";
        default: return "Undefined Error";
    }
}

std::string ModbusTCPClient::ConnectionStateToString(ConnectionState state)
{
    switch (state) {
        case DISCONNECTED: return "Disconnected";
        case CONNECTING: return "Connecting";
        case CONNECTED: return "Connected";
        case ERROR_STATE: return "Error State";
        default: return "Unknown State";
    }
}

void ModbusTCPClient::SetConnectionState(ConnectionState newState, const std::string& message)
{
    ConnectionState oldState = m_connectionState.exchange(newState);
    
    if (oldState != newState && m_stateCallback) {
        m_stateCallback(oldState, newState, message);
    }
}

bool ModbusTCPClient::IsModbusContextValid() const
{
    return m_modbusContext != nullptr;
}

ModbusTCPClient::Result ModbusTCPClient::ConvertLibmodbusError() const
{
    return ERROR_UNKNOWN;
}

void ModbusTCPClient::SetLastError(const std::string& error)
{
    m_lastError = error;
}