#include "PLCModbusClient.h"
#include "VrLog.h"
#include <cstring>
#include <chrono>

PLCModbusClient::PLCModbusClient()
    : m_plcClient(nullptr)
    , m_plcPort(502)
    , m_lastPhotoRequestState(false)
    , m_lastConnectedState(false)
    , m_pollRunning(false)
    , m_photoRequestPaused(false)
    , m_pollIntervalMs(1000)
    , m_shutdownRequested(false)
    , m_autoReconnect(true)
    , m_reconnectInterval(2000)
    , m_plcReconnectAttempts(0)
{
}

PLCModbusClient::~PLCModbusClient()
{
    Shutdown();
}

bool PLCModbusClient::Initialize(const std::string& plcIP, int plcPort)
{
    RegisterConfig regConfig;
    return Initialize(plcIP, plcPort, regConfig);
}

bool PLCModbusClient::Initialize(const std::string& plcIP, int plcPort, const RegisterConfig& regConfig)
{
    // 如果已经在运行，先停止
    if (m_pollRunning) {
        StopPolling();
    }

    m_plcIP = plcIP;
    m_plcPort = plcPort;
    m_registerConfig = regConfig;
    m_shutdownRequested = false;
    m_plcReconnectAttempts = 0;
    m_lastPhotoRequestState = false;
    m_lastConnectedState = false;

    LOG_INFO("Link PLC ip : %s, port : %d\n", plcIP.c_str(), plcPort);
    LOG_INFO("PLCModbusClient Initialize: PhotoRequest=%d, DataComplete=%d, CoordStart=%d\n",
             m_registerConfig.addrPhotoRequest, m_registerConfig.addrDataComplete,
             m_registerConfig.addrCoordDataStart);

    bool connected = false;
    bool createFailed = false;

    // 创建 PLC 客户端并连接
    if (!plcIP.empty()) {
        std::lock_guard<std::mutex> locker(m_mutex);

        // 清理旧的客户端
        if (m_plcClient) {
            m_plcClient->Disconnect();
            delete m_plcClient;
            m_plcClient = nullptr;
        }

        if (!IYModbusTCPClient::CreateInstance(&m_plcClient, plcIP, plcPort)) {
            LOG_ERROR("Failed to create PLC Modbus client\n");
            createFailed = true;
        } else {
            m_plcClient->SetTimeout(1000);
            auto result = m_plcClient->Connect();
            if (result != IYModbusTCPClient::SUCCESS) {
                LOG_WARNING("Failed to connect to PLC at %s:%d, error: %s\n",
                            m_plcIP.c_str(), m_plcPort, m_plcClient->GetLastError().c_str());
            } else {
                LOG_INFO("Connected to PLC at %s:%d\n", m_plcIP.c_str(), m_plcPort);
                connected = true;
            }
        }
    }

    // 在锁外调用回调，避免死锁
    if (createFailed) {
        notifyError("PLC客户端创建失败");
    }

    m_lastConnectedState = connected;
    notifyConnectionStateChanged(connected);

    return true;
}

void PLCModbusClient::Shutdown()
{
    // 防止重复调用
    bool expected = false;
    if (!m_shutdownRequested.compare_exchange_strong(expected, true)) {
        return;
    }

    LOG_INFO("PLCModbusClient shutdown requested\n");

    // 停止轮询线程
    StopPolling();

    // 清理 PLC 客户端
    {
        std::lock_guard<std::mutex> locker(m_mutex);
        if (m_plcClient) {
            m_plcClient->Disconnect();
            delete m_plcClient;
            m_plcClient = nullptr;
        }
    }

    LOG_INFO("PLCModbusClient shutdown complete\n");
}

void PLCModbusClient::StartPolling(int intervalMs)
{
    if (m_shutdownRequested) {
        LOG_WARNING("Cannot start polling: shutdown requested\n");
        return;
    }

    if (m_pollRunning) {
        LOG_WARNING("Polling already running\n");
        return;
    }

    m_pollIntervalMs = intervalMs;
    m_pollRunning = true;
    m_pollThread = std::thread(&PLCModbusClient::pollThreadFunc, this);
    LOG_INFO("PLC polling started, interval: %d ms\n", intervalMs);
}

void PLCModbusClient::StopPolling()
{
    if (!m_pollRunning) {
        return;
    }

    m_pollRunning = false;
    if (m_pollThread.joinable()) {
        m_pollThread.join();
    }
    LOG_INFO("PLC polling stopped\n");
}

void PLCModbusClient::SetPhotoTriggerCallback(PhotoTriggerCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_photoCallback = callback;
}

void PLCModbusClient::SetConnectionStateCallback(ConnectionStateCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_connectionStateCallback = callback;
}

void PLCModbusClient::SetErrorCallback(ErrorCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_errorCallback = callback;
}

void PLCModbusClient::SetReconnectingCallback(ReconnectingCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_reconnectingCallback = callback;
}

void PLCModbusClient::SetReconnectInterval(int intervalMs)
{
    m_reconnectInterval = intervalMs;
}

void PLCModbusClient::SetAutoReconnect(bool enable)
{
    m_autoReconnect = enable;
}

void PLCModbusClient::pollThreadFunc()
{
    LOG_INFO("Poll thread started\n");

    int reconnectCounter = 0;

    while (m_pollRunning && !m_shutdownRequested) {
        // 检查连接状态
        bool currentConnected = checkConnection();

        // 连接状态变化时通知
        if (currentConnected != m_lastConnectedState) {
            m_lastConnectedState = currentConnected;
            notifyConnectionStateChanged(currentConnected);

            // 重连成功后重置状态
            if (currentConnected) {
                m_lastPhotoRequestState = false;
                m_plcReconnectAttempts = 0;
                reconnectCounter = 0;
                LOG_INFO("PLC reconnected successfully\n");
            }
        }

        if (!currentConnected) {
            // 未连接时，尝试重连
            if (m_autoReconnect) {
                reconnectCounter++;
                int reconnectThreshold = (m_pollIntervalMs > 0) ? (m_reconnectInterval / m_pollIntervalMs) : 30;
                if (reconnectCounter >= reconnectThreshold) {
                    reconnectCounter = 0;
                    m_plcReconnectAttempts++;

                    notifyReconnecting("PLC", m_plcReconnectAttempts);
                    LOG_INFO("Attempting to reconnect PLC (attempt %d)...\n", m_plcReconnectAttempts);
                    tryConnectPLC();
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(m_pollIntervalMs));
            continue;
        }

        // 已连接，检查是否暂停了拍照请求轮询
        if (m_photoRequestPaused) {
            // 暂停期间不读取拍照请求，只维持连接
            std::this_thread::sleep_for(std::chrono::milliseconds(m_pollIntervalMs));
            continue;
        }

        // 读取拍照请求寄存器
        int requestValue = readPhotoRequest();
        if (requestValue >= 0) {
            // 边沿检测：只在 0->1 变化时触发拍照
            bool currentState = (requestValue == 1);
            if (currentState && !m_lastPhotoRequestState) {
                LOG_INFO("Photo request detected (D1000=1)\n");
                notifyPhotoRequested(1);
            }
            m_lastPhotoRequestState = currentState;
        } else {
            // 读取失败，可能是连接断开，主动断开连接以触发重连
            LOG_WARNING("Failed to read photo request, disconnecting to trigger reconnect\n");
            disconnectPLC();
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(m_pollIntervalMs));
    }

    LOG_INFO("Poll thread exited\n");
}

bool PLCModbusClient::checkConnection()
{
    std::lock_guard<std::mutex> locker(m_mutex);
    return m_plcClient && m_plcClient->IsConnected();
}

bool PLCModbusClient::tryConnectPLC()
{
    std::lock_guard<std::mutex> locker(m_mutex);

    if (!m_plcClient) {
        if (!IYModbusTCPClient::CreateInstance(&m_plcClient, m_plcIP, m_plcPort)) {
            LOG_ERROR("Failed to create PLC Modbus client\n");
            return false;
        }
        m_plcClient->SetTimeout(1000);
    }

    auto result = m_plcClient->Connect();
    if (result != IYModbusTCPClient::SUCCESS) {
        LOG_WARNING("Failed to connect to PLC at %s:%d, error: %s\n",
                    m_plcIP.c_str(), m_plcPort, m_plcClient->GetLastError().c_str());
        return false;
    }

    LOG_INFO("Connected to PLC at %s:%d\n", m_plcIP.c_str(), m_plcPort);
    return true;
}

void PLCModbusClient::disconnectPLC()
{
    std::lock_guard<std::mutex> locker(m_mutex);

    if (m_plcClient) {
        m_plcClient->Disconnect();
        LOG_INFO("PLC disconnected for reconnection\n");
    }
}

int PLCModbusClient::readPhotoRequest()
{
    std::lock_guard<std::mutex> locker(m_mutex);

    if (!m_plcClient || !m_plcClient->IsConnected()) {
        return -1;
    }

    std::vector<uint16_t> values;
    auto result = m_plcClient->ReadHoldingRegisters(m_registerConfig.addrPhotoRequest, 1, values);

    if (result != IYModbusTCPClient::SUCCESS || values.empty()) {
        return -1;
    }

    return static_cast<int>(values[0]);
}

bool PLCModbusClient::ClearPhotoRequest()
{
    std::lock_guard<std::mutex> locker(m_mutex);

    if (!m_plcClient || !m_plcClient->IsConnected()) {
        LOG_ERROR("PLC not connected, cannot clear photo request\n");
        return false;
    }

    auto result = m_plcClient->WriteSingleRegister(m_registerConfig.addrPhotoRequest, 0);
    if (result != IYModbusTCPClient::SUCCESS) {
        LOG_ERROR("Failed to clear photo request (addr %d): %s\n",
                  m_registerConfig.addrPhotoRequest, m_plcClient->GetLastError().c_str());
        return false;
    }

    LOG_INFO("Cleared photo request (addr %d = 0)\n", m_registerConfig.addrPhotoRequest);
    return true;
}

void PLCModbusClient::floatToRegisters(float value, uint16_t& high, uint16_t& low)
{
    uint32_t bits;
    std::memcpy(&bits, &value, sizeof(float));
    high = static_cast<uint16_t>((bits >> 16) & 0xFFFF);
    low = static_cast<uint16_t>(bits & 0xFFFF);
}

bool PLCModbusClient::SendCoordinateToPLC(const CoordinateData& coord, int pointIndex)
{
    std::lock_guard<std::mutex> locker(m_mutex);

    if (!m_plcClient || !m_plcClient->IsConnected()) {
        LOG_ERROR("PLC not connected, cannot send coordinate\n");
        return false;
    }

    if (pointIndex < 0 || pointIndex >= MAX_POINTS) {
        LOG_ERROR("Invalid point index: %d (valid: 0-%d)\n", pointIndex, MAX_POINTS - 1);
        return false;
    }

    int startAddr = m_registerConfig.addrCoordDataStart + pointIndex * REGS_PER_POINT;

    std::vector<uint16_t> registers(REGS_PER_POINT);
    floatToRegisters(coord.x, registers[0], registers[1]);
    floatToRegisters(coord.y, registers[2], registers[3]);
    floatToRegisters(coord.z, registers[4], registers[5]);
    floatToRegisters(coord.pitch, registers[6], registers[7]);
    floatToRegisters(coord.roll, registers[8], registers[9]);
    floatToRegisters(coord.yaw, registers[10], registers[11]);

    auto result = m_plcClient->WriteMultipleRegisters(startAddr, registers);
    if (result != IYModbusTCPClient::SUCCESS) {
        LOG_ERROR("Failed to write coordinate to PLC (addr %d): %s\n",
                  startAddr, m_plcClient->GetLastError().c_str());
        return false;
    }

    LOG_INFO("Sent point[%d] (addr %d): X=%.3f, Y=%.3f, Z=%.3f, P=%.3f, R=%.3f, Y=%.3f\n",
             pointIndex, startAddr, coord.x, coord.y, coord.z, coord.pitch, coord.roll, coord.yaw);
    return true;
}

bool PLCModbusClient::SendCoordinatesToPLC(const std::vector<CoordinateData>& coords)
{
    std::lock_guard<std::mutex> locker(m_mutex);

    if (!m_plcClient || !m_plcClient->IsConnected()) {
        LOG_ERROR("PLC not connected, cannot send coordinates\n");
        return false;
    }

    int pointCount = std::min(static_cast<int>(coords.size()), MAX_POINTS);
    if (pointCount == 0) {
        LOG_WARNING("No coordinates to send\n");
        return true;
    }

    std::vector<uint16_t> registers(pointCount * REGS_PER_POINT);

    for (int i = 0; i < pointCount; i++) {
        const CoordinateData& coord = coords[i];
        int offset = i * REGS_PER_POINT;
        floatToRegisters(coord.x,   registers[offset + 0], registers[offset + 1]);
        floatToRegisters(coord.y,   registers[offset + 2], registers[offset + 3]);
        floatToRegisters(coord.z,   registers[offset + 4], registers[offset + 5]);
        floatToRegisters(coord.roll, registers[offset + 6], registers[offset + 7]);
        floatToRegisters(coord.pitch, registers[offset + 8], registers[offset + 9]);
        floatToRegisters(coord.yaw, registers[offset + 10], registers[offset + 11]);
    }

    auto result = m_plcClient->WriteMultipleRegisters(m_registerConfig.addrCoordDataStart, registers);
    if (result != IYModbusTCPClient::SUCCESS) {
        LOG_ERROR("Failed to write coordinates to PLC (addr %d): %s\n",
                  m_registerConfig.addrCoordDataStart, m_plcClient->GetLastError().c_str());
        return false;
    }

    LOG_INFO("Sent %d points to PLC (addr %d-%d)\n",
             pointCount, m_registerConfig.addrCoordDataStart,
             m_registerConfig.addrCoordDataStart + pointCount * REGS_PER_POINT - 1);
    return true;
}

bool PLCModbusClient::NotifyDataComplete()
{
    std::lock_guard<std::mutex> locker(m_mutex);

    if (!m_plcClient || !m_plcClient->IsConnected()) {
        LOG_ERROR("PLC not connected, cannot notify data complete\n");
        return false;
    }

    auto result = m_plcClient->WriteSingleRegister(m_registerConfig.addrDataComplete, 1);
    if (result != IYModbusTCPClient::SUCCESS) {
        LOG_ERROR("Failed to notify data complete (addr %d): %s\n",
                  m_registerConfig.addrDataComplete, m_plcClient->GetLastError().c_str());
        return false;
    }

    LOG_INFO("Notified data complete (addr %d = 1)\n", m_registerConfig.addrDataComplete);
    return true;
}

bool PLCModbusClient::IsPLCConnected() const
{
    std::lock_guard<std::mutex> locker(m_mutex);
    return m_plcClient && m_plcClient->IsConnected();
}

void PLCModbusClient::PausePhotoRequestPolling()
{
    m_photoRequestPaused = true;
    LOG_INFO("Photo request polling paused\n");
}

void PLCModbusClient::ResumePhotoRequestPolling()
{
    m_photoRequestPaused = false;
    LOG_INFO("Photo request polling resumed\n");
}

bool PLCModbusClient::IsPhotoRequestPollingPaused() const
{
    return m_photoRequestPaused;
}

// ========== 安全的回调通知（先复制回调，释放锁后再调用）==========

void PLCModbusClient::notifyConnectionStateChanged(bool connected)
{
    ConnectionStateCallback callback;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        callback = m_connectionStateCallback;
    }
    if (callback) {
        callback(connected);
    }
}

void PLCModbusClient::notifyError(const std::string& errorMsg)
{
    ErrorCallback callback;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        callback = m_errorCallback;
    }
    if (callback) {
        callback(errorMsg);
    }
}

void PLCModbusClient::notifyPhotoRequested(int cameraIndex)
{
    PhotoTriggerCallback callback;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        callback = m_photoCallback;
    }
    if (callback) {
        callback(cameraIndex);
    }
}

void PLCModbusClient::notifyReconnecting(const std::string& device, int attempt)
{
    ReconnectingCallback callback;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        callback = m_reconnectingCallback;
    }
    if (callback) {
        callback(device, attempt);
    }
}