GrabBag/App/BagThreadPosition/BagThreadPositionApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"
#include "bagThreadPositioning_Export.h"
#include <fstream>
#include <QPainter>
#include <QPen>
#include <QColor>

DetectPresenter::DetectPresenter(/* args */)
{
    LOG_DEBUG("DetectPresenter Init algo ver: %s\n", wd_bagThreadPositioningVersion());
}

DetectPresenter::~DetectPresenter()
{
}


int DetectPresenter::DetectScrew(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    DetectionResult& detectionResult)
{
    if (laserLines.empty()) {
        LOG_WARNING("No laser lines data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 获取当前相机的校准参数
    VrCameraPlaneCalibParam cameraCalibParamValue;
    const VrCameraPlaneCalibParam* cameraCalibParam = nullptr;
    if (algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraCalibParamValue)) {
        cameraCalibParam = &cameraCalibParamValue;
    }

    // 保存debug数据
    std::string timeStamp = CVrDateUtils::GetNowTime();
    if(debugParam.enableDebug && debugParam.savePointCloud){
        LOG_INFO("[Algo Thread] Debug mode is enabled, saving point cloud data\n");

        // 获取当前时间戳，格式为YYYYMMDDHHMMSS
        std::string fileName = debugParam.debugOutputPath + "/Laserline_" + std::to_string(cameraIndex) + "_" + timeStamp + ".txt";

        // 直接使用统一格式保存数据
        dataLoader.SaveLaserScanData(fileName, laserLines, laserLines.size(), 0.0, 0, 0);
    }

    int nRet = SUCCESS;

    // 转换为算法需要的XYZ格式
    std::vector<std::vector<SVzNL3DPosition>> xyzData;
    int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, xyzData);
    if (convertResult != SUCCESS || xyzData.empty()) {
        LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 构建扫描信息结构体
    SSX_ScanInfo scanInfo;
    scanInfo.isHorizonScan = algorithmParams.threadParam.isHorizonScan;
    scanInfo.scanFromThreadHead = algorithmParams.threadParam.scanFromThreadHead;
    scanInfo.stitchWidth = algorithmParams.threadParam.stitchWidth;
    scanInfo.operateDist = algorithmParams.threadParam.operateDist;
    scanInfo.mark_diameter = algorithmParams.threadParam.mark_diameter;
    scanInfo.mark_height = algorithmParams.threadParam.mark_height;
    scanInfo.mark_distance = algorithmParams.threadParam.mark_distance;

    SSG_cornerParam cornerParam;
    cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
    cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
    cornerParam.scale = algorithmParams.cornerParam.scale;
    cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
    cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
    cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;

    SSG_treeGrowParam growParam;
    growParam.yDeviation_max = algorithmParams.growParam.yDeviation_max;
    growParam.zDeviation_max = algorithmParams.growParam.zDeviation_max;
    growParam.maxLineSkipNum = algorithmParams.growParam.maxLineSkipNum;
    growParam.maxSkipDistance = algorithmParams.growParam.maxSkipDistance;
    growParam.minLTypeTreeLen = algorithmParams.growParam.minLTypeTreeLen;
    growParam.minVTypeTreeLen = algorithmParams.growParam.minVTypeTreeLen;

    SSG_outlierFilterParam filterParam;
    filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
    filterParam.outlierTh = algorithmParams.filterParam.outlierTh;

    if(debugParam.enableDebug && debugParam.printDetailLog)
    {
        LOG_INFO("[Algo Thread] clibMatrix: \n\t[%.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f] \n\t[ %.3f, %.3f, %.3f, %.3f]\n",
            clibMatrix[0], clibMatrix[1], clibMatrix[2], clibMatrix[3],
            clibMatrix[4], clibMatrix[5], clibMatrix[6], clibMatrix[7],
            clibMatrix[8], clibMatrix[9], clibMatrix[10], clibMatrix[11],
            clibMatrix[12], clibMatrix[13], clibMatrix[14], clibMatrix[15]);

        // 打印拆线参数
        LOG_INFO("[Algo Thread] Thread: isHorizonScan=%s, scanFromThreadHead=%s\n",
            scanInfo.isHorizonScan ? "true" : "false",
            scanInfo.scanFromThreadHead ? "true" : "false");
        LOG_INFO("[Algo Thread] Thread: stitchWidth=%.1f, operateDist=%.1f\n", scanInfo.stitchWidth, scanInfo.operateDist);
        LOG_INFO("[Algo Thread] Thread Mark: diameter=%.3f, height=%.3f, distance=%.3f\n",
            scanInfo.mark_diameter, scanInfo.mark_height, scanInfo.mark_distance);

        // 打印拐角参数
        LOG_INFO("[Algo Thread] Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
            cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap,
            cornerParam.minEndingGap_z, cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);

        // 打印树生长参数
        LOG_INFO("[Algo Thread] Tree Grow: yDeviation_max=%.1f, zDeviation_max=%.1f, maxLineSkipNum=%d, maxSkipDistance=%.1f, minLTypeTreeLen=%.1f, minVTypeTreeLen=%.1f\n",
            growParam.yDeviation_max, growParam.zDeviation_max, growParam.maxLineSkipNum,
            growParam.maxSkipDistance, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);

        // 打印滤波参数
        LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", filterParam.continuityTh, filterParam.outlierTh);
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before wd_bagThreadPositioning \n");

    // 调用拆线定位算法
    std::vector<SSX_bagThreadInfo> threadInfo;
    std::vector<SSX_bagThreadInfo> threadInfo_relative;  // 相对于Mark的坐标
    std::vector<SVzNL3DPoint> output_markCenter;  // Mark中心位置

    // 凸起特征参数（使用配置值）
    SSG_raisedFeatureParam raisedFeaturePara;
    raisedFeaturePara.minJumpZ = algorithmParams.raisedFeatureParam.minJumpZ;
    raisedFeaturePara.minK = algorithmParams.raisedFeatureParam.minK;
    raisedFeaturePara.widthRange.min = algorithmParams.raisedFeatureParam.widthMin;
    raisedFeaturePara.widthRange.max = algorithmParams.raisedFeatureParam.widthMax;

    // 构建平面标定参数
    SSG_planeCalibPara poseCalibPara;
    if (cameraCalibParam && cameraCalibParam->isCalibrated) {
        // 使用实际的标定矩阵
        for (int i = 0; i < 9; i++) {
            poseCalibPara.planeCalib[i] = cameraCalibParam->planeCalib[i];
            poseCalibPara.invRMatrix[i] = cameraCalibParam->invRMatrix[i];
        }
        poseCalibPara.planeHeight = cameraCalibParam->planeHeight;
    } else {
        // 使用单位矩阵（未校准状态）
        double identityMatrix[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        for (int i = 0; i < 9; i++) {
            poseCalibPara.planeCalib[i] = identityMatrix[i];
            poseCalibPara.invRMatrix[i] = identityMatrix[i];
        }
        poseCalibPara.planeHeight = 2600.0;  // 使用默认高度
    }

    wd_bagThreadPositioning(
        xyzData,
        scanInfo,
        poseCalibPara,
        filterParam,
        cornerParam,
        raisedFeaturePara,
        growParam,
        threadInfo,
        threadInfo_relative,
        output_markCenter,
        &errCode);

    LOG_DEBUG("after wd_bagThreadPositioning \n");

    LOG_INFO("wd_bagThreadPositioning: detected %zu threads, %zu marks, err=%d runtime=%.3fms\n",
             threadInfo_relative.size(), output_markCenter.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    // 构建检测结果：生成点云图像
    // 1. 获取所有拆线的线头位置和Mark位置用于可视化
    std::vector<std::vector<SVzNL3DPoint>> objOps;
    std::vector<SVzNL3DPoint> threadPositions;

    for (const auto& thread : threadInfo_relative) {
        threadPositions.push_back(thread.threadPos);
    }

    // 从点云数据生成投影图像（3cm边界）
    detectionResult.image = PointCloudImageUtils::GeneratePointCloudRetPointImage(xyzData, objOps, 0.0);

    // 记录Mark信息用于调试
    if (!output_markCenter.empty() && debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] Detected %zu Mark centers:\n", output_markCenter.size());
        for (size_t i = 0; i < output_markCenter.size(); i++) {
            LOG_INFO("[Algo Thread] Mark %zu: X=%.2f, Y=%.2f, Z=%.2f\n",  i, output_markCenter[i].x, output_markCenter[i].y, output_markCenter[i].z);
        }
    }

    // 在图像上绘制拆线位置点和Mark点
    if (!detectionResult.image.isNull() && !threadInfo.empty()) {
        QPainter painter(&detectionResult.image);
        painter.setRenderHint(QPainter::Antialiasing);

        // 计算点云范围（与PointCloudImageUtils相同的方式）
        double xMin = 1e10, xMax = -1e10, yMin = 1e10, yMax = -1e10;
        for (const auto& line : xyzData) {
            for (const auto& pt : line) {
                if (pt.pt3D.z < 1e-4) continue;
                xMin = std::min(xMin, (double)pt.pt3D.x);
                xMax = std::max(xMax, (double)pt.pt3D.x);
                yMin = std::min(yMin, (double)pt.pt3D.y);
                yMax = std::max(yMax, (double)pt.pt3D.y);
            }
        }


        // 使用与GeneratePointCloudRetPointImage相同的参数
        int imgRows = detectionResult.image.height();
        int imgCols = detectionResult.image.width();
        int x_skip = 50;
        int y_skip = 50;

        // 计算投影比例（与PointCloudImageUtils相同的方式）
        double y_rows = (double)(imgRows - y_skip * 2);
        double x_cols = (double)(imgCols - x_skip * 2);
        double x_scale = (xMax - xMin) / x_cols;
        double y_scale = (yMax - yMin) / y_rows;

        // 使用统一的比例尺
        double scale = std::max(x_scale, y_scale);
        x_scale = scale;
        y_scale = scale;

        // 计算点云在图像中居中的偏移量（与PointCloudImageUtils相同）
        double cloudWidth = (xMax - xMin) / scale;
        double cloudHeight = (yMax - yMin) / scale;
        int x_offset = x_skip + (int)((x_cols - cloudWidth) / 2);
        int y_offset = y_skip + (int)((y_rows - cloudHeight) / 2);

        // 转换3D坐标到图像坐标的lambda函数（使用居中偏移）
        auto toImageCoord = [&](const SVzNL3DPoint& pt) -> QPointF {
            int px = (int)((pt.x - xMin) / x_scale + x_offset);
            int py = (int)((pt.y - yMin) / y_scale + y_offset);
            return QPointF(px, py);
        };

        // 绘制Mark中心点（蓝色大圆点）
        if (!output_markCenter.empty()) {
            QPen markPen(QColor(0, 0, 255), 3);
            painter.setPen(markPen);
            painter.setBrush(QBrush(QColor(0, 0, 255)));
            for (size_t i = 0; i < output_markCenter.size(); i++) {
                QPointF markPt = toImageCoord(output_markCenter[i]);
                painter.drawEllipse(markPt, 5, 5);

                // 标注Mark编号
                painter.setPen(QPen(Qt::cyan, 1));
                painter.drawText(markPt.x() + 10, markPt.y() - 10, QString("M%1").arg(i + 1));
                painter.setPen(markPen);
            }
        }

        // 绘制线头位置和拆点位置
        QPen threadPen(QColor(255, 0, 0), 3);
        QPen operatePen(QColor(0, 255, 0), 3);
        QFont font("Arial", 12, QFont::Bold);
        painter.setFont(font);

        for (size_t idx = 0; idx < threadInfo.size(); idx++) {
            const auto& thread = threadInfo[idx];
            QString indexStr = QString::number(idx + 1);

            // 绘制线头位置（红色）
            painter.setPen(threadPen);
            painter.setBrush(QBrush(QColor(255, 0, 0)));
            QPointF threadPt = toImageCoord(thread.threadPos);
            painter.drawEllipse(threadPt, 3, 3);

            // 在线头下方标注类型，并绘制垂直引导线
            int threadTextY = threadPt.y() + 50;
            painter.setPen(QPen(QColor(255, 0, 0), 1));  // 红色引导线
            painter.drawLine(threadPt.x(), threadPt.y() + 5, threadPt.x(), threadTextY - 12);
            painter.setPen(QPen(Qt::white, 1));
            painter.drawText(threadPt.x() - 15, threadTextY, QString("%1-线").arg(indexStr));

            // 绘制拆点位置（绿色）
            painter.setPen(operatePen);
            painter.setBrush(QBrush(QColor(0, 255, 0)));
            QPointF operatePt = toImageCoord(thread.operatePos);
            painter.drawEllipse(operatePt, 3, 3);

            // 在下刀点上方标注类型，并绘制垂直引导线
            int operateTextY = operatePt.y() - 50;
            painter.setPen(QPen(QColor(0, 255, 0), 1));  // 绿色引导线
            painter.drawLine(operatePt.x(), operatePt.y() - 5, operatePt.x(), operateTextY + 5);
            painter.setPen(QPen(Qt::white, 1));
            painter.drawText(operatePt.x() - 15, operateTextY, QString("%1-刀").arg(indexStr));

            // 绘制连线
            QPen linePen(QColor(255, 255, 0), 2);
            painter.setPen(linePen);
            painter.drawLine(threadPt, operatePt);
        }
    }

    // 转换检测结果为UI显示格式（使用机械臂坐标系数据）
    for (size_t i = 0; i < threadInfo_relative.size(); i++) {
        const auto& thread = threadInfo_relative[i];
        int groupIdx = static_cast<int>(i + 1);  // 组序号从1开始

        // 1. 添加线头位置
        SVzNL3DPoint robotThreadPos;
        CVrConvert::EyeToRobot(thread.threadPos, robotThreadPos, clibMatrix);

        ThreadPosition threadHeadPos;
        threadHeadPos.x          = robotThreadPos.x;
        threadHeadPos.y          = robotThreadPos.y;
        threadHeadPos.z          = robotThreadPos.z;
        threadHeadPos.roll       = thread.rotateAngle;
        threadHeadPos.pitch      = 0.0;
        threadHeadPos.yaw        = 0.0;
        threadHeadPos.type       = ThreadPosType::ThreadHead;
        threadHeadPos.groupIndex = groupIdx;
        detectionResult.positions.push_back(threadHeadPos);

        // 2. 添加下刀位置
        SVzNL3DPoint robotOperatePos;
        CVrConvert::EyeToRobot(thread.operatePos, robotOperatePos, clibMatrix);

        ThreadPosition operatePos;
        operatePos.x          = robotOperatePos.x;
        operatePos.y          = robotOperatePos.y;
        operatePos.z          = robotOperatePos.z;
        operatePos.roll       = thread.rotateAngle;
        operatePos.pitch      = 0.0;
        operatePos.yaw        = 0.0;
        operatePos.type       = ThreadPosType::OperatePos;
        operatePos.groupIndex = groupIdx;
        detectionResult.positions.push_back(operatePos);

        // 保存拆线信息（使用下刀位置）
        ThreadInfo info;
        info.centerX = robotOperatePos.x;
        info.centerY = robotOperatePos.y;
        info.centerZ = robotOperatePos.z;
        info.axialDirX = 0.0;
        info.axialDirY = 0.0;
        info.axialDirZ = 0.0;
        info.rotateAngle = thread.rotateAngle;
        detectionResult.threadInfoList.push_back(info);

        if(debugParam.enableDebug && debugParam.printDetailLog){
            LOG_INFO("[Algo Thread] Thread %zu ThreadPos: X=%.2f, Y=%.2f, Z=%.2f\n", i, thread.threadPos.x, thread.threadPos.y, thread.threadPos.z);
            LOG_INFO("[Algo Thread] Thread %zu OperatePos: X=%.2f, Y=%.2f, Z=%.2f\n", i, thread.operatePos.x, thread.operatePos.y, thread.operatePos.z);
            LOG_INFO("[Algo Thread] Thread %zu Robot ThreadHead: X=%.2f, Y=%.2f, Z=%.2f\n", i, threadHeadPos.x, threadHeadPos.y, threadHeadPos.z);
            LOG_INFO("[Algo Thread] Thread %zu Robot OperatePos: X=%.2f, Y=%.2f, Z=%.2f, Angle=%.2f\n", i, operatePos.x, operatePos.y, operatePos.z, operatePos.roll);
        }
    }

    if(debugParam.enableDebug && debugParam.saveDebugImage){
        // 获取当前时间戳，格式为YYYYMMDDHHMMSS
        std::string fileName = debugParam.debugOutputPath + "/Image_" + std::to_string(cameraIndex) + "_" + timeStamp + ".png";
        LOG_INFO("[Algo Thread] Debug image saved image : %s\n", fileName.c_str());

        // 保存检测结果图片
        if (!detectionResult.image.isNull()) {
            QString qFileName = QString::fromStdString(fileName);
            detectionResult.image.save(qFileName);
        } else {
            LOG_WARNING("[Algo Thread] No valid image to save for debug\n");
        }
    }

    return nRet;
}