GrabBag/App/StatorPosition/StatorPositionApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"
#include "motorStatorPosition_Export.h"
#include "SG_baseAlgo_Export.h"
#include <fstream>
#include <QPainter>
#include <QPen>
#include <QColor>
#include "CoordinateTransform.h"

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

DetectPresenter::~DetectPresenter()
{
}


int DetectPresenter::DetectStatorPosition(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    int eulerOrder,
    int dirVectorInvert,
    StatorPositionDetectionResult& 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);
    }

    // 定子物理参数
    SWD_statorParam statorParam;
    statorParam.statorOuterD = algorithmParams.statorParam.statorOuterD;
    statorParam.statorInnerD = algorithmParams.statorParam.statorInnerD;
    statorParam.statorHeight = algorithmParams.statorParam.statorHeight;
    statorParam.plateThickness = algorithmParams.statorParam.plateThickness;
    statorParam.plateW = algorithmParams.statorParam.plateW;
    statorParam.plateH = algorithmParams.statorParam.plateH;

    // 构造算法参数（使用 cornerParam 而非 lineSegParam）
    SWD_statorPositonParam algoParam;

    // 滤波参数
    algoParam.filterParam.continuityTh = algorithmParams.filterParam.continuityTh;
    algoParam.filterParam.outlierTh = algorithmParams.filterParam.outlierTh;

    // 角点检测参数
    algoParam.cornerParam.minEndingGap = algorithmParams.cornerParam.minEndingGap;
    algoParam.cornerParam.minEndingGap_z = algorithmParams.cornerParam.minEndingGap_z;
    algoParam.cornerParam.scale = algorithmParams.cornerParam.scale;
    algoParam.cornerParam.cornerTh = algorithmParams.cornerParam.cornerTh;
    algoParam.cornerParam.jumpCornerTh_1 = algorithmParams.cornerParam.jumpCornerTh_1;
    algoParam.cornerParam.jumpCornerTh_2 = algorithmParams.cornerParam.jumpCornerTh_2;

    // 树生长参数
    algoParam.growParam.yDeviation_max = algorithmParams.growParam.yDeviation_max;
    algoParam.growParam.zDeviation_max = algorithmParams.growParam.zDeviation_max;
    algoParam.growParam.maxLineSkipNum = algorithmParams.growParam.maxLineSkipNum;
    algoParam.growParam.maxSkipDistance = algorithmParams.growParam.maxSkipDistance;
    algoParam.growParam.minLTypeTreeLen = algorithmParams.growParam.minLTypeTreeLen;
    algoParam.growParam.minVTypeTreeLen = algorithmParams.growParam.minVTypeTreeLen;

    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] StatorParam: outerD=%.1f, innerD=%.1f, height=%.1f, plateThickness=%.1f, plateW=%.1f, plateH=%.1f\n",
            statorParam.statorOuterD, statorParam.statorInnerD, statorParam.statorHeight,
            statorParam.plateThickness, statorParam.plateW, statorParam.plateH);

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

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

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

    // 准备平面校准参数
    SSG_planeCalibPara groundCalibPara;
    if(cameraCalibParam){
        memcpy(groundCalibPara.planeCalib, cameraCalibParam->planeCalib, sizeof(double) * 9);
        memcpy(groundCalibPara.invRMatrix, cameraCalibParam->invRMatrix, sizeof(double) * 9);
        groundCalibPara.planeHeight = cameraCalibParam->planeHeight;
    } else {
        // 使用默认单位矩阵
        double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        memcpy(groundCalibPara.planeCalib, identity, sizeof(double) * 9);
        memcpy(groundCalibPara.invRMatrix, identity, sizeof(double) * 9);
        groundCalibPara.planeHeight = -1.0;
    }

    if(debugParam.enableDebug && debugParam.printDetailLog)
    {
        LOG_INFO("Plane height: %.3f\n", groundCalibPara.planeHeight);
        LOG_INFO("  Plane calibration matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
                 groundCalibPara.planeCalib[0], groundCalibPara.planeCalib[1], groundCalibPara.planeCalib[2],
                 groundCalibPara.planeCalib[3], groundCalibPara.planeCalib[4], groundCalibPara.planeCalib[5],
                 groundCalibPara.planeCalib[6], groundCalibPara.planeCalib[7], groundCalibPara.planeCalib[8]);

        LOG_INFO("  Plane invRMatrix matrix: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n",
                 groundCalibPara.invRMatrix[0], groundCalibPara.invRMatrix[1], groundCalibPara.invRMatrix[2],
                 groundCalibPara.invRMatrix[3], groundCalibPara.invRMatrix[4], groundCalibPara.invRMatrix[5],
                 groundCalibPara.invRMatrix[6], groundCalibPara.invRMatrix[7], groundCalibPara.invRMatrix[8]);
    }

    // 数据预处理：调平和去除地面（使用当前相机的调平参数）
    if(cameraCalibParam){
        LOG_DEBUG("Processing data with plane calibration\n");
        double groundH = -1;
        for(size_t i = 0; i < xyzData.size(); i++){
            wd_lineDataR(xyzData[i], cameraCalibParam->planeCalib, groundH);
        }
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before wd_motorStatorPosition \n");

    // 状态机（每次检测新建）
    SWD_statorGriperState stateMachine;
    memset(&stateMachine, 0, sizeof(SWD_statorGriperState));

    // 调用电机定子定位算法
    std::vector<SWD_statorInnerGrasper> resultObjPositions;
    wd_motorStatorPosition(
        xyzData,
        statorParam,
        groundCalibPara,
        algoParam,
        &stateMachine,
        &errCode,
        resultObjPositions
    );

    LOG_DEBUG("after wd_motorStatorPosition \n");

    LOG_INFO("wd_motorStatorPosition: found %zu stators, err=%d runtime=%.3fms\n", resultObjPositions.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    // 构建手眼标定的齐次变换矩阵（使用 Eigen-based CoordinateTransform）
    CTHomogeneousMatrix calibMatrix;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            calibMatrix.at(i, j) = clibMatrix[i * 4 + j];
        }
    }

    // 确定欧拉角顺序（eulerOrder 直接映射到 CTEulerOrder 枚举值）
    CTEulerOrder ctEulerOrder = static_cast<CTEulerOrder>(eulerOrder);

    // 构建用于可视化的点数组
    std::vector<SVzNL3DPoint> objCenterPoints;

    // 处理每个定子的检测结果
    for (size_t i = 0; i < resultObjPositions.size(); i++) {
        const SWD_statorInnerGrasper& obj = resultObjPositions[i];

        // 算法输出的位姿（角度为度），转换为弧度构建相机坐标系下的位姿
        CTCameraPose cameraPose(
            obj.opCenter.x, obj.opCenter.y, obj.opCenter.z,
            CCoordinateTransform::degToRad(obj.opCenter.x_roll),
            CCoordinateTransform::degToRad(obj.opCenter.y_pitch),
            CCoordinateTransform::degToRad(obj.opCenter.z_yaw));

        // 使用 Eigen-based CoordinateTransform 进行 Eye-to-Hand 坐标变换
        CTRobotPose targetPose;
        CCoordinateTransform::sixAxisEyeToHandCalcGraspPose(cameraPose, calibMatrix, ctEulerOrder, targetPose);

        // 将机器人坐标系下的位姿添加到positions列表（弧度转为度）
        StatorGraspPosition pos;
        pos.objID = obj.objID;
        pos.x = targetPose.x;
        pos.y = targetPose.y;
        pos.z = targetPose.z;
        pos.roll  = CCoordinateTransform::radToDeg(targetPose.rx);
        pos.pitch = CCoordinateTransform::radToDeg(targetPose.ry);
        pos.yaw   = CCoordinateTransform::radToDeg(targetPose.rz);

        // 应用方向向量反向配置
        switch (dirVectorInvert) {
            case DIR_INVERT_XY:
                pos.roll  = -pos.roll;
                pos.pitch = -pos.pitch;
                break;
            case DIR_INVERT_XZ:
                pos.roll  = -pos.roll;
                pos.yaw   = -pos.yaw;
                break;
            case DIR_INVERT_YZ:
                pos.pitch = -pos.pitch;
                pos.yaw   = -pos.yaw;
                break;
            case DIR_INVERT_NONE:
            default:
                break;
        }

        detectionResult.positions.push_back(pos);

        // 添加中心点到可视化列表
        SVzNL3DPoint centerPt;
        centerPt.x = obj.opCenter.x;
        centerPt.y = obj.opCenter.y;
        centerPt.z = obj.opCenter.z;
        objCenterPoints.push_back(centerPt);

        if(debugParam.enableDebug && debugParam.printDetailLog){
            LOG_INFO("[Algo Thread] Stator[%zu] objID=%d Eye Coords: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
                    i, obj.objID,
                    obj.opCenter.x, obj.opCenter.y, obj.opCenter.z,
                    obj.opCenter.x_roll, obj.opCenter.y_pitch, obj.opCenter.z_yaw);
            LOG_INFO("[Algo Thread] Stator[%zu] Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, R=%.4f, P=%.4f, Y=%.4f\n",
                    i, pos.x, pos.y, pos.z,
                    pos.roll, pos.pitch, pos.yaw);
        }
    }

    // 从点云数据生成投影图像（单色灰色点云 + 红色抓取点标记）
    {
        // 固定图像尺寸
        int imgRows = 992;
        int imgCols = 1056;
        int x_skip = 50;
        int y_skip = 50;

        // 计算点云范围
        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);
            }
        }

        // 计算投影比例
        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;
        if (x_scale < y_scale)
            x_scale = y_scale;
        else
            y_scale = x_scale;

        // 创建图像
        QImage image(imgCols, imgRows, QImage::Format_RGB888);
        image.fill(Qt::black);
        QPainter painter(&image);
        painter.setRenderHint(QPainter::Antialiasing);

        // 绘制点云数据 - 使用单色灰色
        QColor grayColor(150, 150, 150);
        for (const auto& scanLine : xyzData) {
            for (const auto& point : scanLine) {
                if (point.pt3D.z < 1e-4) continue;

                int px = (int)((point.pt3D.x - xMin) / x_scale + x_skip);
                int py = (int)((point.pt3D.y - yMin) / y_scale + y_skip);

                if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
                    painter.setPen(QPen(grayColor, 1));
                    painter.drawPoint(px, py);
                }
            }
        }

        // 绘制抓取点标记 - 使用红色
        if (!objCenterPoints.empty()) {
            QColor markerColor(255, 0, 0);  // 红色
            painter.setPen(QPen(markerColor, 1));
            painter.setBrush(QBrush(markerColor));

            for (size_t i = 0; i < objCenterPoints.size(); i++) {
                const SVzNL3DPoint& pt = objCenterPoints[i];

                // 跳过全0的点
                if (fabs(pt.x) < 0.0001 && fabs(pt.y) < 0.0001 && fabs(pt.z) < 0.0001) {
                    continue;
                }

                int px = (int)((pt.x - xMin) / x_scale + x_skip);
                int py = (int)((pt.y - yMin) / y_scale + y_skip);

                if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
                    // 绘制圆点标记
                    int circleSize = 6;
                    painter.drawEllipse(px - circleSize/2, py - circleSize/2, circleSize, circleSize);

                    // 绘制编号
                    painter.setPen(QPen(Qt::white, 1));
                    QFont font("Arial", 14, QFont::Bold);
                    painter.setFont(font);
                    painter.drawText(px + 8, py + 6, QString("%1").arg(i + 1));

                    // 恢复画笔
                    painter.setPen(QPen(markerColor, 1));
                    painter.setBrush(QBrush(markerColor));
                }
            }
        }

        detectionResult.image = image;
    }

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