#include "DetectPresenter.h"

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

SG_APISHARED_EXPORT void wd_getLineVFlaws(
    std::vector<SVzNL3DPosition>& line,
    int lineIndex,
    SWD_valleyParam valleyParam,
    std::vector<SSG_basicFeatureGap>& valleys);

SG_APISHARED_EXPORT void sg_lineGapsGrowing(
    int lineIndex,
    bool isLastLine,
    std::vector<SSG_basicFeatureGap>& features,
    std::vector<SSG_gapFeatureTree>& trees,
    SSG_treeGrowParam growParam);

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());
}

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)
{
    if (laserLines.empty()) {
        detectionResult.message = QStringLiteral("扫描数据为空");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    std::vector<std::vector<SVzNL3DPosition>> scanLines;
    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;
    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;
        wd_getLineVFlaws(scanLines[lineIndex], static_cast<int>(lineIndex),
                         params.valleyParam, 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;
}