#include "DetectPresenter.h"

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

namespace {

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)
{
    PointCloudCanvas canvas = PointCloudCanvas::Create(scanLines);
    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::ResetIncrementalData()
{
    std::lock_guard<std::mutex> lock(m_incrementalMutex);
    m_incrementalScanLines.clear();
    m_incrementalLineValleys.clear();
}

bool DetectPresenter::HasIncrementalData() const
{
    std::lock_guard<std::mutex> lock(m_incrementalMutex);
    return !m_incrementalScanLines.empty();
}

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::lock_guard<std::mutex> lock(m_incrementalMutex);
    const int lineIndex = static_cast<int>(m_incrementalScanLines.size());
    wd_getLineVFlaws(scanLine, lineIndex, params.valleyParam,
                     params.detectFlawType, valleys);
    m_incrementalScanLines.push_back(std::move(scanLine));
    m_incrementalLineValleys.push_back(std::move(valleys));
    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;
    std::vector<std::vector<SSG_basicFeatureGap>> preprocessedValleys;
    const bool useIncrementalData = HasIncrementalData();
    if (useIncrementalData) {
        std::lock_guard<std::mutex> lock(m_incrementalMutex);
        scanLines = m_incrementalScanLines;
        preprocessedValleys = m_incrementalLineValleys;
        if (scanLines.empty() || scanLines.size() != preprocessedValleys.size()) {
            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;
    trees.reserve(scanLines.size());
    for (size_t lineIndex = 0; lineIndex < scanLines.size(); ++lineIndex) {
        std::vector<SSG_basicFeatureGap> lineValleys;
        if (useIncrementalData) {
            lineValleys = preprocessedValleys[lineIndex];
        } else {
            wd_getLineVFlaws(scanLines[lineIndex], static_cast<int>(lineIndex),
                             params.valleyParam, params.detectFlawType, lineValleys);
        }
        const bool isLastLine = lineIndex + 1 == scanLines.size();
        sg_lineGapsGrowing(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 = 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 = BuildResultImage(scanLines, weldPaths);
    SaveDebugImage(cameraIndex, debugParam, detectionResult.image);
    return SUCCESS;
}