#include "DetectPresenter.h"

#include "AlgorithmParamConverter.h"
#include "PointCloudFilter.h"
#include "PointCloudImageUtils.h"
#include "SG_baseAlgo_Export.h"
#include "SX_lapWeldDetection_Export.h"
#include "VrDateUtils.h"
#include "VrError.h"
#include "VrLog.h"
#include "VrTimeUtils.h"

#include <QColor>
#include <QCoreApplication>
#include <QDir>
#include <QFont>
#include <QPainter>
#include <QPen>
#include <QRectF>
#include <algorithm>
#include <cmath>
#include <limits>

namespace {

constexpr int kResultImageWidth = 1056;
constexpr int kResultImageHeight = 992;
constexpr int kResultImageMargin = 50;
constexpr double kImageFillRatio = 0.8;
constexpr double kMinimumImageScale = 1e-3;

int ValidPointCount(const std::vector<SVzNL3DPosition>& line)
{
    return static_cast<int>(std::count_if(line.begin(), line.end(), [](const SVzNL3DPosition& point) {
        return point.pt3D.z > 1e-4;
    }));
}

void RemoveEmptyEdgeLines(std::vector<std::vector<SVzNL3DPosition>>& scanLines)
{
    const auto first = std::find_if(scanLines.begin(), scanLines.end(), [](const auto& line) {
        return ValidPointCount(line) > 0;
    });
    const auto last = std::find_if(scanLines.rbegin(), scanLines.rend(), [](const auto& line) {
        return ValidPointCount(line) > 0;
    }).base();

    if (first == scanLines.end()) {
        scanLines.clear();
        return;
    }

    std::vector<std::vector<SVzNL3DPosition>> trimmed(first, last);
    scanLines.swap(trimmed);
}

QImage BuildResultImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                        const std::vector<std::vector<SVzNL3DPoint>>& weldPaths)
{
    // 仅过滤绘图副本，不改变焊缝检测使用的原始点云。
    std::vector<std::vector<SVzNL3DPosition>> filteredScanLines;
    PointCloudFilterParams filterParams;
    filterParams.method = PointCloudFilterMethod::ScanLineContinuity;
    filterParams.continuityThreshold = 20.0;
    filterParams.minSegmentLength = 5;
    PointCloudFilterStats filterStats;
    const bool filterSuccess = PointCloudFilter::Filter(scanLines, filteredScanLines,
                                                        filterParams, &filterStats);
    const auto& imageScanLines = filterSuccess ? filteredScanLines : scanLines;
    LOG_DEBUG("WeldSeam image point filter: input=%zu output=%zu removed=%zu method=%d\n",
              filterStats.inputPointCount, filterStats.outputPointCount,
              filterStats.removedPointCount,
              static_cast<int>(filterParams.method));

    PointCloudCanvas canvas = PointCloudCanvas::Create(imageScanLines);
    if (!canvas.isValid()) return QImage();

    const QColor pathColor(0, 220, 255);
    const QColor pointColor(255, 80, 40);
    int pointIndex = 1;
    for (const auto& path : weldPaths) {
        for (size_t i = 1; i < path.size(); ++i) {
            canvas.drawLine(path[i - 1].x, path[i - 1].y,
                            path[i].x, path[i].y, pathColor, 3);
        }
        for (const auto& point : path) {
            canvas.drawPoint(point.x, point.y, pointColor, 9);
            canvas.drawText(point.x, point.y, QString::number(pointIndex++),
                            pointColor, 13, 7, -7);
        }
    }
    return canvas.image().copy();
}

void SaveDebugImage(int cameraIndex, const VrDebugParam& debugParam, const QImage& image)
{
    if (!debugParam.enableDebug || !debugParam.saveDebugImage || image.isNull()) return;
    const QString outputPath = debugParam.debugOutputPath.empty()
        ? QCoreApplication::applicationDirPath() + QStringLiteral("/debug")
        : QString::fromStdString(debugParam.debugOutputPath);
    QDir dir(outputPath);
    if (!dir.exists() && !dir.mkpath(QStringLiteral("."))) {
        LOG_WARNING("Failed to create WeldSeam debug image directory: %s\n",
                    outputPath.toUtf8().constData());
        return;
    }
    const QString fileName = dir.filePath(
        QStringLiteral("WeldSeam_%1_%2.png")
            .arg(cameraIndex)
            .arg(QString::fromStdString(CVrDateUtils::GetNowTime())));
    if (!image.save(fileName)) {
        LOG_WARNING("Failed to save WeldSeam debug image: %s\n",
                    fileName.toUtf8().constData());
    }
}

} // namespace

DetectPresenter::DetectPresenter()
{
    LOG_INFO("lapWeldDetection version: %s\n", wd_weldSeamAlgoVersion());
}

void DetectPresenter::ResetIncrementalImage()
{
    m_incrementalImage = QImage();
    m_imageXMin = 0.0;
    m_imageYMin = 0.0;
    m_imageScale = 1.0;
    m_observedXMin = 0.0;
    m_observedXMax = 0.0;
    m_observedYMin = 0.0;
    m_observedYMax = 0.0;
    m_incrementalImageValid = false;
    m_imageInputPointCount = 0;
    m_imageOutputPointCount = 0;
}

void DetectPresenter::ResetIncrementalData()
{
    std::lock_guard<std::mutex> lock(m_incrementalMutex);
    m_pendingLineValleys.clear();
    m_incrementalFeatureTrees.clear();
    m_incrementalLineCount = 0;
    ResetIncrementalImage();
}

bool DetectPresenter::HasIncrementalData() const
{
    std::lock_guard<std::mutex> lock(m_incrementalMutex);
    return m_incrementalLineCount > 0;
}

void DetectPresenter::AddIncrementalImageLine(
    const std::vector<SVzNL3DPosition>& scanLine)
{
    double lineXMin = std::numeric_limits<double>::max();
    double lineXMax = std::numeric_limits<double>::lowest();
    double lineYMin = std::numeric_limits<double>::max();
    double lineYMax = std::numeric_limits<double>::lowest();
    bool hasValidPoint = false;
    for (const auto& point : scanLine) {
        const SVzNL3DPoint& value = point.pt3D;
        if (value.z <= 1e-4 || !std::isfinite(value.x) || !std::isfinite(value.y)) {
            continue;
        }
        lineXMin = std::min(lineXMin, value.x);
        lineXMax = std::max(lineXMax, value.x);
        lineYMin = std::min(lineYMin, value.y);
        lineYMax = std::max(lineYMax, value.y);
        hasValidPoint = true;
    }
    if (!hasValidPoint) {
        return;
    }

    if (!m_incrementalImageValid) {
        m_observedXMin = lineXMin;
        m_observedXMax = lineXMax;
        m_observedYMin = lineYMin;
        m_observedYMax = lineYMax;
    } else {
        m_observedXMin = std::min(m_observedXMin, lineXMin);
        m_observedXMax = std::max(m_observedXMax, lineXMax);
        m_observedYMin = std::min(m_observedYMin, lineYMin);
        m_observedYMax = std::max(m_observedYMax, lineYMax);
    }

    const double drawableWidth = kResultImageWidth - kResultImageMargin * 2;
    const double drawableHeight = kResultImageHeight - kResultImageMargin * 2;
    const double currentXMax = m_imageXMin + drawableWidth * m_imageScale;
    const double currentYMax = m_imageYMin + drawableHeight * m_imageScale;
    const bool needsResize = !m_incrementalImageValid ||
        m_observedXMin < m_imageXMin || m_observedXMax > currentXMax ||
        m_observedYMin < m_imageYMin || m_observedYMax > currentYMax;

    if (needsResize) {
        const double observedWidth = std::max(m_observedXMax - m_observedXMin,
                                              kMinimumImageScale);
        const double observedHeight = std::max(m_observedYMax - m_observedYMin,
                                               kMinimumImageScale);
        const double newScale = std::max({
            observedWidth / (drawableWidth * kImageFillRatio),
            observedHeight / (drawableHeight * kImageFillRatio),
            kMinimumImageScale
        });
        const double centerX = (m_observedXMin + m_observedXMax) * 0.5;
        const double centerY = (m_observedYMin + m_observedYMax) * 0.5;
        const double newXMin = centerX - drawableWidth * newScale * 0.5;
        const double newYMin = centerY - drawableHeight * newScale * 0.5;

        QImage resizedImage(kResultImageWidth, kResultImageHeight, QImage::Format_RGB888);
        resizedImage.fill(Qt::black);
        if (m_incrementalImageValid && !m_incrementalImage.isNull()) {
            // 扫描范围扩展时只重投影固定画布，避免保留历史点云重新绘制。
            const double oldWorldLeft =
                m_imageXMin - kResultImageMargin * m_imageScale;
            const double oldWorldTop =
                m_imageYMin - kResultImageMargin * m_imageScale;
            const QRectF targetRect(
                (oldWorldLeft - newXMin) / newScale + kResultImageMargin,
                (oldWorldTop - newYMin) / newScale + kResultImageMargin,
                kResultImageWidth * m_imageScale / newScale,
                kResultImageHeight * m_imageScale / newScale);
            QPainter resizePainter(&resizedImage);
            resizePainter.drawImage(targetRect, m_incrementalImage);
        }
        m_incrementalImage = std::move(resizedImage);
        m_imageXMin = newXMin;
        m_imageYMin = newYMin;
        m_imageScale = newScale;
        m_incrementalImageValid = true;
    }

    QPainter painter(&m_incrementalImage);
    painter.setPen(QPen(QColor(150, 150, 150), 1));
    for (const auto& point : scanLine) {
        const SVzNL3DPoint& value = point.pt3D;
        if (value.z <= 1e-4 || !std::isfinite(value.x) || !std::isfinite(value.y)) {
            continue;
        }
        const int pixelX = static_cast<int>(
            (value.x - m_imageXMin) / m_imageScale + kResultImageMargin);
        const int pixelY = static_cast<int>(
            (value.y - m_imageYMin) / m_imageScale + kResultImageMargin);
        if (pixelX >= 0 && pixelX < kResultImageWidth &&
            pixelY >= 0 && pixelY < kResultImageHeight) {
            painter.drawPoint(pixelX, pixelY);
        }
    }
}

QImage DetectPresenter::BuildIncrementalResultImage(
    const std::vector<std::vector<SVzNL3DPoint>>& weldPaths) const
{
    std::lock_guard<std::mutex> lock(m_incrementalMutex);
    if (!m_incrementalImageValid || m_incrementalImage.isNull()) {
        return QImage();
    }

    QImage resultImage = m_incrementalImage.copy();
    QPainter painter(&resultImage);
    painter.setRenderHint(QPainter::Antialiasing);
    const QColor pathColor(0, 220, 255);
    const QColor pointColor(255, 80, 40);
    const auto project = [this](const SVzNL3DPoint& point) {
        return QPoint(
            static_cast<int>((point.x - m_imageXMin) / m_imageScale +
                             kResultImageMargin),
            static_cast<int>((point.y - m_imageYMin) / m_imageScale +
                             kResultImageMargin));
    };

    int pointIndex = 1;
    for (const auto& path : weldPaths) {
        painter.setPen(QPen(pathColor, 3));
        for (std::size_t index = 1; index < path.size(); ++index) {
            painter.drawLine(project(path[index - 1]), project(path[index]));
        }
        painter.setPen(QPen(pointColor, 1));
        painter.setBrush(QBrush(pointColor));
        painter.setFont(QFont(QStringLiteral("Arial"), 13, QFont::Bold));
        for (const auto& point : path) {
            const QPoint pixel = project(point);
            painter.drawEllipse(pixel.x() - 4, pixel.y() - 4, 9, 9);
            painter.drawText(pixel.x() + 7, pixel.y() - 7,
                             QString::number(pointIndex++));
        }
    }
    LOG_DEBUG("WeldSeam streaming image filter: input=%zu output=%zu removed=%zu\n",
              m_imageInputPointCount, m_imageOutputPointCount,
              m_imageInputPointCount >= m_imageOutputPointCount
                  ? m_imageInputPointCount - m_imageOutputPointCount : 0);
    return resultImage;
}

bool DetectPresenter::ProcessDetectionLine(
    EVzResultDataType dataType,
    const SVzLaserLineData& lineData,
    const VrAlgorithmParams& algorithmParams,
    const SSG_planeCalibPara& groundCalibParam,
    LaserDataLoader& dataLoader)
{
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> oneLine;
    oneLine.emplace_back(dataType, lineData);
    std::vector<std::vector<SVzNL3DPosition>> converted;
    if (dataLoader.ConvertToSVzNL3DPosition(oneLine, converted) != SUCCESS ||
        converted.empty() || converted.front().empty() ||
        ValidPointCount(converted.front()) == 0) {
        return false;
    }

    std::vector<SVzNL3DPosition> scanLine = std::move(converted.front());
    sx_lineDataR(scanLine, groundCalibParam.planeCalib, -1.0);
    const WeldSeamAlgorithmParams params = AlgorithmParamConverter::ToAlgorithmParams(algorithmParams);
    std::vector<SSG_basicFeatureGap> valleys;
    std::vector<SVzNL3DPosition> filteredImageLine;
    PointCloudFilterParams filterParams;
    filterParams.method = PointCloudFilterMethod::ScanLineContinuity;
    filterParams.continuityThreshold = 20.0;
    filterParams.minSegmentLength = 5;
    PointCloudFilterStats filterStats;
    const bool filterSuccess = PointCloudFilter::Filter(scanLine, filteredImageLine, filterParams, &filterStats);
    std::lock_guard<std::mutex> lock(m_incrementalMutex);
    const int lineIndex = static_cast<int>(m_incrementalLineCount);
    wd_getLineVFlaws(scanLine, lineIndex, params.valleyParam, params.detectFlawType, valleys);
    if (m_incrementalLineCount > 0)
    {
        // 当前线到达后立即归并上一条，避免逐线特征缓存随扫描长度增长。
        sg_lineGapsGrowing_treeFirst(lineIndex - 1, false, m_pendingLineValleys, m_incrementalFeatureTrees, params.treeGrowParam);
    }
    m_pendingLineValleys = std::move(valleys);
    ++m_incrementalLineCount;
    m_imageInputPointCount += filterStats.inputPointCount;
    m_imageOutputPointCount += filterSuccess ? filterStats.outputPointCount : filterStats.inputPointCount;
    AddIncrementalImageLine(filterSuccess ? filteredImageLine : scanLine);
    return true;
}

QString DetectPresenter::GetAlgoVersion()
{
    return QString::fromUtf8(wd_weldSeamAlgoVersion());
}

int DetectPresenter::DetectWeldSeam(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const SSG_planeCalibPara& groundCalibParam,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    DetectionResult& detectionResult)
{
    std::vector<std::vector<SVzNL3DPosition>> scanLines;
    const bool useIncrementalData = HasIncrementalData();
    if (useIncrementalData)
    {
        std::lock_guard<std::mutex> lock(m_incrementalMutex);
        if (m_incrementalLineCount == 0) {
            detectionResult.message = QStringLiteral("扫描数据为空");
            return ERR_CODE(DEV_DATA_INVALID);
        }
    } else {
        const int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, scanLines);
        RemoveEmptyEdgeLines(scanLines);
        if (convertResult != SUCCESS || scanLines.empty()) {
            detectionResult.message = QStringLiteral("扫描数据转换失败");
            return ERR_CODE(DEV_DATA_INVALID);
        }
    }

    const WeldSeamAlgorithmParams params = AlgorithmParamConverter::ToAlgorithmParams(algorithmParams);

    if(debugParam.enableDebug && debugParam.printDetailLog){
        LOG_DEBUG("valleyParam: chkWinWidth=%.3f maxWidth=%.3f minHeight=%.3f\n",
                  params.valleyParam.chkWin_width, params.valleyParam.maxWidth,
                  params.valleyParam.minHeight);
        LOG_DEBUG("weldSeamParam: refPoints=%d\n", params.weldRefPoints);
        LOG_DEBUG("treeGrowParam: skipLines=%d yDev=%.3f skipDist=%.3f zDev=%.3f minL=%.3f minV=%.3f\n",
                  params.treeGrowParam.maxLineSkipNum,            
                  params.treeGrowParam.yDeviation_max, params.treeGrowParam.maxSkipDistance,
                  params.treeGrowParam.zDeviation_max, params.treeGrowParam.minLTypeTreeLen,
                  params.treeGrowParam.minVTypeTreeLen);

        LOG_DEBUG("planeCalib: planeHeight=%.6f\n", groundCalibParam.planeHeight);
        LOG_DEBUG("planeCalib=[%.6f %.6f %.6f; %.6f %.6f %.6f; %.6f %.6f %.6f]\n",
                 groundCalibParam.planeCalib[0], groundCalibParam.planeCalib[1], groundCalibParam.planeCalib[2],
                 groundCalibParam.planeCalib[3], groundCalibParam.planeCalib[4], groundCalibParam.planeCalib[5],
                 groundCalibParam.planeCalib[6], groundCalibParam.planeCalib[7], groundCalibParam.planeCalib[8]);
        LOG_DEBUG("invRMatrix=[%.6f %.6f %.6f; %.6f %.6f %.6f; %.6f %.6f %.6f]\n",
                 groundCalibParam.invRMatrix[0], groundCalibParam.invRMatrix[1], groundCalibParam.invRMatrix[2],
                 groundCalibParam.invRMatrix[3], groundCalibParam.invRMatrix[4], groundCalibParam.invRMatrix[5],
                 groundCalibParam.invRMatrix[6], groundCalibParam.invRMatrix[7], groundCalibParam.invRMatrix[8]);
    }

    CVrTimeUtils timer;
    if (!useIncrementalData) {
        for (auto& line : scanLines) {
            sx_lineDataR(line, groundCalibParam.planeCalib, -1.0);
        }
    }

    std::vector<SSG_gapFeatureTree> trees;
    if (useIncrementalData) {
        std::lock_guard<std::mutex> lock(m_incrementalMutex);
        // STOP_SCAN 时才标记最后一线，保持与离线批处理完全一致的收尾语义。
        sg_lineGapsGrowing_treeFirst(static_cast<int>(m_incrementalLineCount - 1), true,
                           m_pendingLineValleys, m_incrementalFeatureTrees, params.treeGrowParam);
        trees.swap(m_incrementalFeatureTrees);
        m_pendingLineValleys.clear();
        m_incrementalLineCount = 0;
    }
    else
    {
        trees.reserve(scanLines.size());
        for (std::size_t lineIndex = 0; lineIndex < scanLines.size(); ++lineIndex)
        {
            std::vector<SSG_basicFeatureGap> lineValleys;
            wd_getLineVFlaws(scanLines[lineIndex], static_cast<int>(lineIndex),
                             params.valleyParam, params.detectFlawType, lineValleys);
            const bool isLastLine = lineIndex + 1 == scanLines.size();
            sg_lineGapsGrowing_treeFirst(static_cast<int>(lineIndex), isLastLine,
                               lineValleys, trees, params.treeGrowParam);
        }
    }

    int algorithmError = 0;
    std::vector<SVzNL3DPoint> weldOps;
    std::vector<SVzNL3DPosition> weldRawPoints;
    BQ_getWeldSeamPose(trees, params.weldRefPoints,
                       weldOps, weldRawPoints, &algorithmError);

    const size_t weldPointCount = weldOps.size();
    std::vector<std::vector<SVzNL3DPoint>> weldPaths;
    if (!weldOps.empty()) weldPaths.push_back(weldOps);
    LOG_INFO("BQ_getWeldSeamPose: err=%d trees=%zu points=%zu runtime=%.3fms\n",
             algorithmError, trees.size(), weldPointCount,
             timer.GetElapsedTimeInMilliSec());

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_DEBUG("[Algo Thread] BQ weld result: errorCode=%d treeCount=%zu pointCount=%zu\n",
                  algorithmError, trees.size(), weldPointCount);
        for (size_t pointIndex = 0; pointIndex < weldOps.size(); ++pointIndex) {
            const auto& point = weldOps[pointIndex];
            LOG_DEBUG("[Algo Thread] weldOps[%zu]: x=%.6f y=%.6f z=%.6f\n",
                      pointIndex, point.x, point.y, point.z);
        }
    }

    if (algorithmError != 0 || weldPointCount == 0) {
        detectionResult.success = false;
        detectionResult.errorCode = algorithmError != 0 ? algorithmError : ERR_CODE(DEV_DATA_INVALID);
        detectionResult.message = QStringLiteral("未检测到有效焊缝");
        detectionResult.image = useIncrementalData
            ? BuildIncrementalResultImage(weldPaths)
            : BuildResultImage(scanLines, weldPaths);
        return detectionResult.errorCode;
    }

    detectionResult.positions.reserve(weldPointCount);
    for (const auto& point : weldOps) {
        detectionResult.positions.emplace_back(point.x, point.y, point.z);
    }
    detectionResult.success = true;
    detectionResult.errorCode = 0;
    detectionResult.message = QStringLiteral("焊缝检测成功");
    detectionResult.image = useIncrementalData
        ? BuildIncrementalResultImage(weldPaths)
        : BuildResultImage(scanLines, weldPaths);
    SaveDebugImage(cameraIndex, debugParam, detectionResult.image);
    return SUCCESS;
}