#include "WeldSeamPresenter.h"

#include "VrError.h"
#include "VrLog.h"

#include <QApplication>
#include <algorithm>
#include <array>
#include <cstring>
#include <vector>

namespace {

constexpr uint16_t kCommandAddress = 0;
constexpr uint16_t kStatusAddress = 1;
constexpr uint16_t kPointCountAddress = 2;
constexpr uint16_t kPointDataAddress = 3;
constexpr size_t kMaxModbusPoints = 40;
constexpr size_t kRegistersPerPoint = 6; // X/Y/Z: 3 float32

void FloatToRegisters(float value, uint16_t& high, uint16_t& low)
{
    uint32_t raw = 0;
    std::memcpy(&raw, &value, sizeof(value));
    high = static_cast<uint16_t>(raw >> 16);
    low = static_cast<uint16_t>(raw & 0xffff);
}

} // namespace

void WeldSeamPresenter::SendTCPResult(const DetectionResult& result)
{
    if (!m_pTCPServer || !m_pTCPServer->IsConnected()) return;
    if (!result.success || result.positions.empty()) {
        m_pTCPServer->SendMessage(QStringLiteral("RESULT_NG"));
        return;
    }

    QString packet = QStringLiteral("RESULT_OK_%1")
                         .arg(static_cast<int>(result.positions.size()));
    for (const auto& point : result.positions) {
        packet += QStringLiteral("_%1_%2_%3")
            .arg(point.x, 0, 'f', 3)
            .arg(point.y, 0, 'f', 3)
            .arg(point.z, 0, 'f', 3);
    }
    m_pTCPServer->SendMessage(packet);
}

void WeldSeamPresenter::PublishModbusResult(const DetectionResult& result)
{
    if (!IsModbusServerRunning()) return;
    const size_t count = result.success
        ? std::min(result.positions.size(), kMaxModbusPoints) : 0;
    const uint16_t countRegister = static_cast<uint16_t>(count);
    WriteModbusRegisters(kPointCountAddress, &countRegister, 1);

    std::vector<uint16_t> registers(kMaxModbusPoints * kRegistersPerPoint, 0);
    for (size_t i = 0; i < count; ++i) {
        const float values[3] = {
            static_cast<float>(result.positions[i].x),
            static_cast<float>(result.positions[i].y),
            static_cast<float>(result.positions[i].z)
        };
        for (size_t axis = 0; axis < 3; ++axis) {
            const size_t offset = i * kRegistersPerPoint + axis * 2;
            FloatToRegisters(values[axis], registers[offset], registers[offset + 1]);
        }
    }
    WriteModbusRegisters(kPointDataAddress, registers.data(),
                         static_cast<uint16_t>(registers.size()));
    UpdateModbusStatus(result.success && count > 0 ? 3 : 4);
}

void WeldSeamPresenter::ResetModbusResults()
{
    if (!IsModbusServerRunning()) return;
    std::vector<uint16_t> zeros(1 + kMaxModbusPoints * kRegistersPerPoint, 0);
    WriteModbusRegisters(kPointCountAddress, zeros.data(), static_cast<uint16_t>(zeros.size()));
}

void WeldSeamPresenter::UpdateModbusStatus(uint16_t status)
{
    if (IsModbusServerRunning()) WriteModbusRegisters(kStatusAddress, &status, 1);
}

int WeldSeamPresenter::InitTCPServer()
{
    if (m_pTCPServer) return SUCCESS;
    m_pTCPServer = new WeldSeamTCPProtocol();
    if (QApplication::instance()) {
        m_pTCPServer->moveToThread(QApplication::instance()->thread());
    }

    connect(m_pTCPServer, &WeldSeamTCPProtocol::ConnectionChanged,
            this, &WeldSeamPresenter::OnTCPConnectionChanged);
    connect(m_pTCPServer, &WeldSeamTCPProtocol::StartScanRequested, this, [this]() {
        const QString response = StartDynamicScan(-1);
        if (m_pTCPServer) m_pTCPServer->SendMessage(response);
    });
    connect(m_pTCPServer, &WeldSeamTCPProtocol::StopScanRequested, this, [this]() {
        const QString response = StopDynamicScanAndDetect();
        if (m_pTCPServer) m_pTCPServer->SendMessage(response);
    });

    const uint16_t port = m_pConfigManager
        ? m_pConfigManager->GetConfigResult().tcpPort : 7800;
    if (!m_pTCPServer->StartServer(port)) {
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
        return ERR_CODE(DEV_OPEN_ERR);
    }
    m_tcpPort = port;
    return SUCCESS;
}

void WeldSeamPresenter::StopTCPServer()
{
    if (!m_pTCPServer) return;
    m_pTCPServer->StopServer();
    delete m_pTCPServer;
    m_pTCPServer = nullptr;
    m_bTCPConnected = false;
}

void WeldSeamPresenter::OnTCPConnectionChanged(bool connected)
{
    m_bTCPConnected = connected;
    if (!connected && m_scanState.load() == ScanState::Scanning) {
        CancelDynamicScan();
    }
    if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
        status->OnRobotConnectionChanged(connected);
        status->OnStatusUpdate(connected ? "TCP客户端已连接" : "TCP客户端已断开");
    }
}