#include "WeldSeamPresenter.h"

#include "DetectPresenter.h"
#include "PathManager.h"
#include "SX_lapWeldDetection_Export.h"
#include "Version.h"
#include "VrError.h"
#include "VrLog.h"
#include "VrTimeUtils.h"

#include <QDateTime>
#include <QFile>
#include <QMetaObject>
#include <cstring>

WeldSeamPresenter::WeldSeamPresenter(QObject* parent)
    : BasePresenter(parent)
{
}

WeldSeamPresenter::~WeldSeamPresenter()
{
    DeinitApp();
}

int WeldSeamPresenter::InitApp()
{
    LOG_INFO("Start WeldSeamApp version: %s\n", WELDSEAM_VERSION_STRING);
    SetWorkStatus(WorkStatus::InitIng);

    m_pDetectPresenter = new DetectPresenter();

    // 将随程序发布的默认配置只在首次启动时落到用户配置目录。
    const QString configPath = PathManager::GetInstance().GetConfigFilePath();
    if (!QFile::exists(configPath) &&
        !QFile::copy(QStringLiteral(":/app/default_config.xml"), configPath)) {
        LOG_WARNING("Failed to install default WeldSeam config: %s\n",
                    configPath.toUtf8().constData());
    }
    m_pConfigManager = new ConfigManager();
    if (!m_pConfigManager || !m_pConfigManager->Initialize()) {
        LOG_ERROR("WeldSeam configuration initialization failed\n");
        return ERR_CODE(DEV_CONFIG_ERR);
    }

    const ConfigResult config = m_pConfigManager->GetConfigResult();
    SetDebugParam(config.debugParam);
    std::vector<DeviceInfo> cameras = config.cameraList;

    // 动态扫描：使用 VzNLSDK，但明确关闭相机摆动电机。
    InitCamera(cameras, false, false);

    if (InitTCPServer() != SUCCESS) {
        LOG_ERROR("WeldSeam TCP server initialization failed\n");
    }
    CheckAndUpdateWorkStatus();
    return SUCCESS;
}

void WeldSeamPresenter::DeinitApp()
{
    if (m_deinitialized.exchange(true)) return;

    CancelDynamicScan();
    BasePresenter::StopDetection();
    StopTCPServer();

    delete m_pDetectPresenter;
    m_pDetectPresenter = nullptr;
    delete m_pConfigManager;
    m_pConfigManager = nullptr;
}

int WeldSeamPresenter::InitAlgoParams()
{
    if (!m_pConfigManager) return ERR_CODE(DEV_CONFIG_ERR);
    const ConfigResult config = m_pConfigManager->GetConfigResult();
    SetDebugParam(config.debugParam);
    const auto& p = config.algorithmParams;
    LOG_INFO("WeldSeam BQ params loaded: valleyWindow=%.3f valleyMaxWidth=%.3f "
             "valleyMinHeight=%.3f refs=%d skipLines=%d yDev=%.3f skipDist=%.3f "
             "zDev=%.3f minL=%.3f minV=%.3f\n",
             p.valleyParam.chkWinWidth, p.valleyParam.maxWidth,
             p.valleyParam.minHeight, p.weldSeamParam.weldRefPoints,
             p.treeGrowParam.maxLineSkipNum, p.treeGrowParam.yDeviationMax,
             p.treeGrowParam.maxSkipDistance, p.treeGrowParam.zDeviationMax,
             p.treeGrowParam.minLTypeTreeLen, p.treeGrowParam.minVTypeTreeLen);
    LOG_INFO("WeldSeam flaw type=%d\n",
             static_cast<int>(p.weldSeamParam.detectFlawType));
    return SUCCESS;
}

VzNL_OnNotifyStatusCBEx WeldSeamPresenter::GetCameraStatusCallback()
{
    return &WeldSeamPresenter::DynamicCameraStatusCallback;
}

void WeldSeamPresenter::DynamicCameraStatusCallback(
    EVzDeviceWorkStatus status, void* extData, unsigned int dataLength, void* userData)
{
    Q_UNUSED(extData);
    Q_UNUSED(dataLength);
    auto* context = static_cast<CameraCallbackContext*>(userData);
    if (!context || !context->presenter) {
        LOG_ERROR("WeldSeam dynamic camera status callback has invalid context\n");
        return;
    }

    auto* presenter = static_cast<WeldSeamPresenter*>(context->presenter);
    switch (status) {
    case keDeviceWorkStatus_Offline:
        presenter->OnCameraStatusChanged(context->cameraIndex, false);
        break;
    case keDeviceWorkStatus_Eye_Reconnect:
        presenter->OnCameraStatusChanged(context->cameraIndex, true);
        break;
    case keDeviceWorkStatus_Device_Swing_Finish:
        // 动态相机的扫描结束只能由 STOP 命令确认；设备完成通知不启动算法。
        LOG_DEBUG("WeldSeam camera %d reported scan finish; waiting for STOP command\n",
                  context->cameraIndex);
        break;
    default:
        break;
    }
}

QString WeldSeamPresenter::StartDynamicScan(int cameraIndex)
{
    ScanState expected = ScanState::Ready;
    if (!m_scanState.compare_exchange_strong(expected, ScanState::Scanning)) {
        return QStringLiteral("ERROR_BUSY");
    }
    if (!m_bCameraConnected) {
        m_scanState = ScanState::Ready;
        UpdateModbusStatus(4);
        return QStringLiteral("ERROR_NOT_READY");
    }

    ResetModbusResults();
    UpdateModbusStatus(1);
    const int result = BasePresenter::StartDetection(cameraIndex, false);
    if (result != SUCCESS) {
        m_scanState = ScanState::Ready;
        UpdateModbusStatus(4);
        SetWorkStatus(WorkStatus::Error);
        return QStringLiteral("ERROR_NOT_READY");
    }

    MarkCachedResultPending(m_currentCameraIndex);
    OnStatusUpdate("动态扫描已开始，等待外部运动结束");
    return QStringLiteral("START_ACK");
}

QString WeldSeamPresenter::StopDynamicScanAndDetect()
{
    ScanState expected = ScanState::Scanning;
    if (!m_scanState.compare_exchange_strong(expected, ScanState::Calculating)) {
        return expected == ScanState::Calculating
            ? QStringLiteral("ERROR_BUSY") : QStringLiteral("ERROR_NOT_READY");
    }

    const int arrayIndex = m_currentCameraIndex - 1;
    if (arrayIndex < 0 || arrayIndex >= static_cast<int>(m_vrEyeDeviceList.size()) ||
        !m_vrEyeDeviceList[arrayIndex].second) {
        m_scanState = ScanState::Ready;
        UpdateModbusStatus(4);
        SetWorkStatus(WorkStatus::Error);
        return QStringLiteral("ERROR_NOT_READY");
    }

    const int stopResult = m_vrEyeDeviceList[arrayIndex].second->StopDetect();
    if (stopResult != SUCCESS) {
        LOG_ERROR("VzNL dynamic scan stop failed: %d\n", stopResult);
        // 设备未确认停止前不允许开始新任务；保留 Scanning 以便重试 STOP。
        m_scanState = ScanState::Scanning;
        UpdateModbusStatus(4);
        SetWorkStatus(WorkStatus::Error);
        return QStringLiteral("ERROR_INTERNAL_ERROR");
    }

    // StopDetect 成功后才检查缓存。空扫描仍先返回 STOP_ACK，
    // 随后把失败状态写入结果缓存，等待 GET_RET 查询。
    if (!HasPendingDetectionData()) {
        UpdateModbusStatus(2);
        OnStatusUpdate("动态扫描已结束，未采集到有效数据");
        QMetaObject::invokeMethod(this, [this]() {
            CompleteFailedScan(QStringLiteral("扫描数据为空"), ERR_CODE(DEV_DATA_INVALID));
        }, Qt::QueuedConnection);
        return QStringLiteral("STOP_ACK");
    }

    // 非摆动模式不会产生 Swing_Finish；显式唤醒基类算法线程。
    UpdateModbusStatus(2);
    m_algoDetectTaskPending.store(true);
    m_algoDetectCondition.notify_one();
    OnStatusUpdate("动态扫描已结束，正在计算焊缝");
    return QStringLiteral("STOP_ACK");
}

void WeldSeamPresenter::CancelDynamicScan()
{
    ScanState expected = ScanState::Scanning;
    if (!m_scanState.compare_exchange_strong(expected, ScanState::Ready)) return;

    const int arrayIndex = m_currentCameraIndex - 1;
    if (arrayIndex >= 0 && arrayIndex < static_cast<int>(m_vrEyeDeviceList.size()) &&
        m_vrEyeDeviceList[arrayIndex].second) {
        m_vrEyeDeviceList[arrayIndex].second->StopDetect();
    }
    ClearDetectionDataCache();
    ClearCachedResult(m_currentCameraIndex);
    UpdateModbusStatus(m_bCameraConnected ? 0 : 4);
    SetWorkStatus(m_bCameraConnected ? WorkStatus::Ready : WorkStatus::Error);
}

void WeldSeamPresenter::CompleteFailedScan(const QString& message, int errorCode)
{
    ScanState expected = ScanState::Calculating;
    if (!m_scanState.compare_exchange_strong(expected, ScanState::Ready)) return;

    DetectionResult result;
    result.cameraIndex = m_currentCameraIndex;
    result.success = false;
    result.errorCode = errorCode;
    result.message = message;
    StoreCachedResult(result);

    if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
        status->OnDetectionResult(result);
        status->OnStatusUpdate(result.message.toStdString());
    }
    PublishModbusResult(result);
    SetWorkStatus(WorkStatus::Error);
    // 结果状态落定后通知主控，可以开始查询本次检测结果。
    NotifyDetectionCompleted();
}

int WeldSeamPresenter::ProcessAlgoDetection(
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
{
    DetectionResult result;
    result.cameraIndex = m_currentCameraIndex;

    int code = ERR_CODE(DEV_NOT_FIND);
    if (m_pDetectPresenter && m_pConfigManager) {
        const ConfigResult config = m_pConfigManager->GetConfigResult();
        // Live WeldSeam scans are preprocessed line-by-line. The base cache is
        // only populated in debug mode for persistence, so release that copy
        // before asking DetectPresenter to consume its incremental data.
        if (m_pDetectPresenter->HasIncrementalData() && !detectionDataCache.empty()) {
            m_dataLoader.FreeLaserScanData(detectionDataCache);
            detectionDataCache.clear();
        }
        code = m_pDetectPresenter->DetectWeldSeam(
            m_currentCameraIndex, detectionDataCache, config.algorithmParams,
            GetCameraCalibParam(m_currentCameraIndex), config.debugParam,
            m_dataLoader, result);
    } else {
        result.success = false;
        result.message = QStringLiteral("算法或配置未初始化");
    }

    if (code != SUCCESS) {
        result.success = false;
        if (result.errorCode == 0) result.errorCode = code;
    }

    StoreCachedResult(result);
    DetectionResult outputResult;
    int outputCode = code;
    if (!TransformDetectionResult(result, m_currentCameraIndex, outputResult) &&
        result.success) {
        outputCode = outputResult.errorCode;
    }

    if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
        status->OnDetectionResult(outputResult);
        status->OnStatusUpdate(outputResult.message.toStdString());
    }

    PublishModbusResult(outputResult);
    m_scanState = ScanState::Ready;
    // 算法线程只通知任务结束，机械臂坐标仍由 GET_RET_n 按需获取。
    NotifyDetectionCompleted();
    return outputCode;
}

bool WeldSeamPresenter::OnDetectionData(EVzResultDataType dataType,
                                         SVzLaserLineData* laserData)
{
    if (!laserData || !m_pDetectPresenter || !m_pConfigManager ||
        dataType != keResultDataType_Position) {
        return false;
    }

    const ConfigResult config = m_pConfigManager->GetConfigResult();
    const bool processed = m_pDetectPresenter->ProcessDetectionLine(
        dataType, *laserData, config.algorithmParams,
        GetCameraCalibParam(m_currentCameraIndex), m_dataLoader);
    if (!processed) {
        LOG_WARNING("WeldSeam failed to preprocess one point-cloud line\n");
        return true;
    }

    // 单线处理完成后立即释放 SDK 数据，长距离扫描不再深拷贝完整点云。
    return true;
}

bool WeldSeamPresenter::HasPendingDetectionData() const
{
    return m_pDetectPresenter && m_pDetectPresenter->HasIncrementalData();
}

void WeldSeamPresenter::ClearDetectionDataCache()
{
    BasePresenter::ClearDetectionDataCache();
    if (m_pDetectPresenter) m_pDetectPresenter->ResetIncrementalData();
}

int WeldSeamPresenter::LoadAndDetect(const QString& fileName)
{
    ScanState expected = ScanState::Ready;
    if (!m_scanState.compare_exchange_strong(expected, ScanState::Calculating)) {
        return ERR_CODE(DEV_BUSY);
    }
    const int result = LoadDebugDataAndDetect(fileName.toStdString());
    if (result != SUCCESS) {
        m_scanState = ScanState::Ready;
        SetWorkStatus(WorkStatus::Error);
    }
    return result;
}

void WeldSeamPresenter::ReconnectCamera()
{
    TryReconnectCameras();
}

VrAlgorithmParams WeldSeamPresenter::GetAlgorithmParams() const
{
    return m_pConfigManager ? m_pConfigManager->GetConfigResult().algorithmParams
                            : VrAlgorithmParams{};
}

void WeldSeamPresenter::SetAlgorithmParams(const VrAlgorithmParams& params)
{
    if (!m_pConfigManager) return;
    SystemConfig config = m_pConfigManager->GetConfig();
    config.configResult.algorithmParams = params;
    m_pConfigManager->UpdateFullConfig(config);
}

QString WeldSeamPresenter::GetAlgoVersion() const
{
    return DetectPresenter::GetAlgoVersion();
}

void WeldSeamPresenter::OnConfigChanged(const ConfigResult& configResult)
{
    SetDebugParam(configResult.debugParam);
    InitAlgoParams();
    if (configResult.tcpPort != m_tcpPort) {
        StopTCPServer();
        if (InitTCPServer() != SUCCESS) {
            OnStatusUpdate("TCP服务端口更新失败");
        }
    }
    OnStatusUpdate("配置已更新");
}

SSG_planeCalibPara WeldSeamPresenter::GetCameraCalibParam(int cameraIndex) const
{
    SSG_planeCalibPara result{};
    const double identity[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
    std::memcpy(result.planeCalib, identity, sizeof(identity));
    std::memcpy(result.invRMatrix, identity, sizeof(identity));
    result.planeHeight = -1.0;
    if (!m_pConfigManager) return result;

    const auto& list = m_pConfigManager->GetConfigResult().algorithmParams.planeCalibParam.cameraCalibParams;
    for (const auto& item : list) {
        if (item.cameraIndex == cameraIndex && item.isCalibrated) {
            std::memcpy(result.planeCalib, item.planeCalib, sizeof(result.planeCalib));
            std::memcpy(result.invRMatrix, item.invRMatrix, sizeof(result.invRMatrix));
            result.planeHeight = item.planeHeight;
            break;
        }
    }
    return result;
}

bool WeldSeamPresenter::CalculatePlaneCalibration(
    const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData,
    double planeCalib[9], double& planeHeight, double invRMatrix[9])
{
    LaserDataLoader loader;
    std::vector<std::vector<SVzNL3DPosition>> xyzData;
    if (loader.ConvertToSVzNL3DPosition(scanData, xyzData) != SUCCESS || xyzData.empty()) return false;
    const SSG_planeCalibPara result = sx_getBaseCalibPara(xyzData);
    std::memcpy(planeCalib, result.planeCalib, sizeof(result.planeCalib));
    std::memcpy(invRMatrix, result.invRMatrix, sizeof(result.invRMatrix));
    planeHeight = result.planeHeight;
    return true;
}

bool WeldSeamPresenter::SaveLevelingResults(double planeCalib[9], double planeHeight,
                                             double invRMatrix[9], int cameraIndex,
                                             const QString& cameraName)
{
    if (!m_pConfigManager || cameraIndex <= 0) return false;
    SystemConfig config = m_pConfigManager->GetConfig();
    VrCameraPlaneCalibParam item;
    item.cameraIndex = cameraIndex;
    item.cameraName = cameraName.toStdString();
    item.planeHeight = planeHeight;
    item.isCalibrated = true;
    std::memcpy(item.planeCalib, planeCalib, sizeof(item.planeCalib));
    std::memcpy(item.invRMatrix, invRMatrix, sizeof(item.invRMatrix));
    config.configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(item);
    return m_pConfigManager->UpdateFullConfig(config) &&
           m_pConfigManager->SaveConfigToFile(PathManager::GetInstance().GetConfigFilePath().toStdString());
}

bool WeldSeamPresenter::LoadLevelingResults(int cameraIndex, const QString& cameraName,
                                             double planeCalib[9], double& planeHeight,
                                             double invRMatrix[9])
{
    Q_UNUSED(cameraName);
    if (!m_pConfigManager) return false;
    VrCameraPlaneCalibParam item;
    if (!m_pConfigManager->GetConfigResult().algorithmParams.planeCalibParam
             .GetCameraCalibParam(cameraIndex, item) || !item.isCalibrated) return false;
    std::memcpy(planeCalib, item.planeCalib, sizeof(item.planeCalib));
    std::memcpy(invRMatrix, item.invRMatrix, sizeof(item.invRMatrix));
    planeHeight = item.planeHeight;
    return true;
}

void WeldSeamPresenter::CheckAndUpdateWorkStatus()
{
    if (!m_bCameraConnected) {
        SetWorkStatus(WorkStatus::Error);
    } else if (m_scanState.load() != ScanState::Ready) {
        SetWorkStatus(WorkStatus::Working);
    } else {
        SetWorkStatus(WorkStatus::Ready);
    }
}

void WeldSeamPresenter::OnCameraStatusChanged(int cameraIndex, bool connected)
{
    if (cameraIndex <= 0 ||
        cameraIndex > static_cast<int>(m_cameraConnectionStates.size())) {
        LOG_WARNING("WeldSeam ignored camera status for invalid index %d\n", cameraIndex);
        return;
    }

    int statusSlot = 0;
    bool slotConnected = connected;
    bool anyCameraConnected = false;
    {
        std::lock_guard<std::mutex> lock(m_cameraStatusMutex);
        m_cameraConnectionStates[static_cast<size_t>(cameraIndex - 1)] = connected;

        // 逻辑通道1/2、3/4分别复用两个物理相机状态槽。
        statusSlot = (cameraIndex - 1) / 2 + 1;
        const int beginIndex = (statusSlot - 1) * 2;
        int endIndex = beginIndex + 2;
        if (endIndex > m_logicalCameraCount) {
            endIndex = m_logicalCameraCount;
        }
        slotConnected = false;
        for (int index = beginIndex; index < endIndex; ++index) {
            slotConnected = slotConnected ||
                            m_cameraConnectionStates[static_cast<size_t>(index)];
        }

        for (int index = 0; index < m_logicalCameraCount; ++index) {
            anyCameraConnected = anyCameraConnected ||
                                 m_cameraConnectionStates[static_cast<size_t>(index)];
        }
    }

    m_bCameraConnected = anyCameraConnected;
    if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
        if (statusSlot == 1) {
            status->OnCamera1StatusChanged(slotConnected);
        } else if (statusSlot == 2) {
            status->OnCamera2StatusChanged(slotConnected);
        }
    }

    CheckAndUpdateWorkStatus();
    if (!connected && cameraIndex == m_currentCameraIndex &&
        m_scanState.load() == ScanState::Scanning) {
        QMetaObject::invokeMethod(this, [this]() { CancelDynamicScan(); }, Qt::QueuedConnection);
    }
}

void WeldSeamPresenter::OnWorkStatusChanged(WorkStatus statusValue)
{
    if (!m_modbusRegistersInitialized && statusValue == WorkStatus::Ready &&
        IsModbusServerRunning()) {
        const uint16_t zero = 0;
        WriteModbusRegisters(0, &zero, 1);
        ResetModbusResults();
        UpdateModbusStatus(0);
        m_modbusRegistersInitialized = true;
    }
    if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) status->OnWorkStatusChanged(statusValue);
}

void WeldSeamPresenter::OnCameraCountChanged(int count)
{
    int effectiveCameraCount = count;
    {
        std::lock_guard<std::mutex> lock(m_cameraStatusMutex);
        if (effectiveCameraCount < 1) {
            effectiveCameraCount = 1;
        } else if (effectiveCameraCount >
                   static_cast<int>(m_cameraConnectionStates.size())) {
            effectiveCameraCount = static_cast<int>(m_cameraConnectionStates.size());
        }
        m_logicalCameraCount = effectiveCameraCount;
        m_cameraConnectionStates.fill(false);
    }
    {
        std::lock_guard<std::mutex> lock(m_resultCacheMutex);
        m_cachedDetectionResults.fill(CachedDetectionResult());
    }
    m_bCameraConnected = false;
    if (count > static_cast<int>(m_cameraConnectionStates.size())) {
        LOG_WARNING("WeldSeam supports at most %zu cameras; configured count is %d\n",
                    m_cameraConnectionStates.size(), count);
    }
    if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
        status->OnCameraCountChanged(effectiveCameraCount);
    }
}

void WeldSeamPresenter::OnStatusUpdate(const std::string& message)
{
    if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) status->OnStatusUpdate(message);
}

void WeldSeamPresenter::OnModbusServerStatusChanged(bool connected)
{
    if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) status->OnRobotConnectionChanged(connected);
}

void WeldSeamPresenter::OnModbusWriteCallback(uint16_t startAddress,
                                               const uint16_t* data, uint16_t count)
{
    if (!data || count == 0 || startAddress != 0) return;
    const uint16_t command = data[0];
    const uint16_t reset = 0;
    WriteModbusRegisters(0, &reset, 1);
    if (command == 1) {
        const QString response = StartDynamicScan(-1);
        if (response != QStringLiteral("START_ACK")) UpdateModbusStatus(4);
    } else if (command == 2) {
        const QString response = StopDynamicScanAndDetect();
        if (response != QStringLiteral("STOP_ACK")) UpdateModbusStatus(4);
    } else if (command != 0) {
        UpdateModbusStatus(4);
    }
}