#include "WeldSeamPresenter.h"

#include "VrConvert.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

int CacheSlotForCamera(int cameraIndex)
{
    return (cameraIndex - 1) / 2;
}

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::NotifyDetectionCompleted()
{
    if (!m_pTCPServer || !m_pTCPServer->IsConnected()) {
        return;
    }
    m_pTCPServer->SendMessage(QStringLiteral("RESULT_READY"));
}

int WeldSeamPresenter::GetLogicalCameraCount()
{
    std::lock_guard<std::mutex> lock(m_cameraStatusMutex);
    return m_logicalCameraCount;
}

void WeldSeamPresenter::MarkCachedResultPending(int cameraIndex)
{
    const int cacheSlot = CacheSlotForCamera(cameraIndex);
    if (cacheSlot < 0 || cacheSlot >= static_cast<int>(m_cachedDetectionResults.size())) {
        return;
    }

    std::lock_guard<std::mutex> lock(m_resultCacheMutex);
    CachedDetectionResult& cached = m_cachedDetectionResults[static_cast<size_t>(cacheSlot)];
    cached = CachedDetectionResult();
    cached.state = ResultCacheState::Pending;
    cached.result.cameraIndex = cameraIndex;
}

void WeldSeamPresenter::ClearCachedResult(int cameraIndex)
{
    const int cacheSlot = CacheSlotForCamera(cameraIndex);
    if (cacheSlot < 0 || cacheSlot >= static_cast<int>(m_cachedDetectionResults.size())) {
        return;
    }

    std::lock_guard<std::mutex> lock(m_resultCacheMutex);
    m_cachedDetectionResults[static_cast<size_t>(cacheSlot)] = CachedDetectionResult();
}

void WeldSeamPresenter::StoreCachedResult(const DetectionResult& result)
{
    const int cacheSlot = CacheSlotForCamera(result.cameraIndex);
    if (cacheSlot < 0 || cacheSlot >= static_cast<int>(m_cachedDetectionResults.size())) {
        LOG_WARNING("WeldSeam ignored result cache for invalid camera %d\n", result.cameraIndex);
        return;
    }

    std::lock_guard<std::mutex> lock(m_resultCacheMutex);
    CachedDetectionResult& cached = m_cachedDetectionResults[static_cast<size_t>(cacheSlot)];
    cached.result = result;
    cached.state = result.success ? ResultCacheState::Ready : ResultCacheState::Failed;
}

bool WeldSeamPresenter::TransformDetectionResult(const DetectionResult& eyeResult,
                                                 int cameraIndex,
                                                 DetectionResult& robotResult) const
{
    robotResult = eyeResult;
    robotResult.cameraIndex = cameraIndex;
    robotResult.positions.clear();
    if (!eyeResult.success) {
        return false;
    }
    if (!m_pConfigManager) {
        robotResult.success = false;
        robotResult.errorCode = ERR_CODE(DEV_NOT_FIND);
        robotResult.message = QStringLiteral("配置未初始化");
        return false;
    }

    const ConfigResult config = m_pConfigManager->GetConfigResult();
    const VrHandEyeCalibMatrix* handEye = config.FindHandEyeMatrix(cameraIndex);
    if (!handEye) {
        robotResult.success = false;
        robotResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
        robotResult.message = QStringLiteral("机械臂通道%1未配置手眼矩阵").arg(cameraIndex);
        LOG_ERROR("WeldSeam camera %d has no hand-eye matrix\n", cameraIndex);
        return false;
    }

    // 每次查询都从相机坐标副本转换，原始缓存可供同组另一机械臂复用。
    robotResult.positions.reserve(eyeResult.positions.size());
    for (const auto& point : eyeResult.positions) {
        const SVzNL3DPoint eyePoint = {point.x, point.y, point.z};
        SVzNL3DPoint robotPoint = {};
        if (!CVrConvert::EyeToRobot(eyePoint, robotPoint, handEye->matrix)) {
            robotResult.success = false;
            robotResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
            robotResult.message = QStringLiteral("手眼坐标转换失败");
            robotResult.positions.clear();
            return false;
        }
        robotResult.positions.emplace_back(robotPoint.x, robotPoint.y, robotPoint.z);
    }
    robotResult.success = true;
    robotResult.errorCode = 0;
    return true;
}

void WeldSeamPresenter::SendCachedTCPResult(int cameraIndex)
{
    if (!m_pTCPServer || !m_pTCPServer->IsConnected()) {
        return;
    }
    if (cameraIndex <= 0 || cameraIndex > GetLogicalCameraCount()) {
        m_pTCPServer->SendMessage(QStringLiteral("ERROR_INVALID_CAMERA"));
        return;
    }

    const int cacheSlot = CacheSlotForCamera(cameraIndex);
    CachedDetectionResult cached;
    {
        std::lock_guard<std::mutex> lock(m_resultCacheMutex);
        cached = m_cachedDetectionResults[static_cast<size_t>(cacheSlot)];
    }

    if (cached.state == ResultCacheState::Empty) {
        m_pTCPServer->SendMessage(QStringLiteral("RESULT_EMPTY"));
        return;
    }
    if (cached.state == ResultCacheState::Pending) {
        m_pTCPServer->SendMessage(QStringLiteral("RESULT_PENDING"));
        return;
    }
    if (cached.state == ResultCacheState::Failed) {
        m_pTCPServer->SendMessage(QStringLiteral("RESULT_NG"));
        return;
    }

    DetectionResult robotResult;
    if (!TransformDetectionResult(cached.result, cameraIndex, robotResult)) {
        m_pTCPServer->SendMessage(QStringLiteral("RESULT_NG"));
        return;
    }
    SendTCPResult(robotResult);
}

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](int cameraIndex)
    {
        if ((cameraIndex != 1 && cameraIndex != 3) ||
            cameraIndex > GetLogicalCameraCount()) {
            if (m_pTCPServer) {
                m_pTCPServer->SendMessage(QStringLiteral("ERROR_INVALID_CAMERA"));
            }
            return;
        }
        const QString response = StartDynamicScan(cameraIndex);
        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);
        }
    });
    connect(m_pTCPServer, &WeldSeamTCPProtocol::GetResultRequested,
            this, &WeldSeamPresenter::SendCachedTCPResult);

    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客户端已断开");
    }
}