GrabBag/App/HolePitPosition/HolePitPositionApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"
#include "workpieceHolePositioning_Export.h"
#include "SG_baseDataType.h"
#include <fstream>
#include "PointCloudImageUtils.h"
#include "CoordinateTransform.h"
#include "VrConvert.h"
#include "VrTimeUtils.h"
#include "VrDateUtils.h"

// 辅助函数：向量叉积
static CTVec3D crossProduct(const CTVec3D& a, const CTVec3D& b) {
    return CTVec3D(
        a.y * b.z - a.z * b.y,
        a.z * b.x - a.x * b.z,
        a.x * b.y - a.y * b.x
    );
}

DetectPresenter::DetectPresenter()
{
    LOG_DEBUG("DetectPresenter Init (HolePitPosition)\n");
}

DetectPresenter::~DetectPresenter()
{
}

std::string DetectPresenter::GetAlgoVersion() const
{
    const char* ver = wd_workpieceHolePositioningVersion();
    return ver ? std::string(ver) : std::string("unknown");
}

int DetectPresenter::DetectHoles(
    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,
    const RobotFlangePose& robotPose,
    HoleDetectionResult& detectionResult)
{
    if (laserLines.empty()) {
        LOG_WARNING("No laser lines data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

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

        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格式（vector<vector<SVzNL3DPosition>>）
    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);
    }

    LOG_INFO("[Algo Thread] Converted %zu scan lines for wd_HolePositioning\n", xyzData.size());

    // ========== 参数转换：VrAlgorithmParams -> 算法结构体 ==========

    // 线段参数
    SSG_lineSegParam lineSegPara;
    lineSegPara.distScale = algorithmParams.lineSegParam.distScale;
    lineSegPara.segGapTh_y = algorithmParams.lineSegParam.segGapTh_y;
    lineSegPara.segGapTh_z = algorithmParams.lineSegParam.segGapTh_z;

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

    // 噪声滤除参数
    SSG_outlierFilterParam filterParam;
    filterParam.continuityTh = algorithmParams.outlierFilterParam.continuityTh;
    filterParam.outlierTh = algorithmParams.outlierFilterParam.outlierTh;

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

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] lineSegPara: distScale=%.2f, segGapTh_y=%.2f, segGapTh_z=%.2f\n",
                 lineSegPara.distScale, lineSegPara.segGapTh_y, lineSegPara.segGapTh_z);
        LOG_INFO("[Algo Thread] cornerParam: cornerTh=%.2f, scale=%.2f, minEndingGap=%.2f, minEndingGap_z=%.2f, jumpCornerTh_1=%.2f, jumpCornerTh_2=%.2f\n",
                 cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap, cornerParam.minEndingGap_z,
                 cornerParam.jumpCornerTh_1, cornerParam.jumpCornerTh_2);
        LOG_INFO("[Algo Thread] filterParam: continuityTh=%.2f, outlierTh=%.2f\n",
                 filterParam.continuityTh, filterParam.outlierTh);
        LOG_INFO("[Algo Thread] growParam: maxLineSkipNum=%d, yDeviation_max=%.2f, maxSkipDistance=%.2f, zDeviation_max=%.2f, minLTypeTreeLen=%.2f, minVTypeTreeLen=%.2f\n",
                 growParam.maxLineSkipNum, growParam.yDeviation_max, growParam.maxSkipDistance,
                 growParam.zDeviation_max, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);

        // 打印手眼标定矩阵
        LOG_INFO("[Algo Thread] Hand-Eye Calibration Matrix:\n");
        for (int r = 0; r < 4; ++r) {
            LOG_INFO("  [%.4f, %.4f, %.4f, %.4f]\n",
                     clibMatrix[r * 4 + 0], clibMatrix[r * 4 + 1], clibMatrix[r * 4 + 2], clibMatrix[r * 4 + 3]);
        }
    }

    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before wd_HolePositioning\n");

    // ========== 调用 wd_HolePositioning 算法 ==========
    std::vector<WD_HolePositionInfo> holePositioning;
    int errCode = 0;
    wd_HolePositioning(xyzData, lineSegPara, cornerParam, filterParam, growParam, holePositioning, &errCode);

    LOG_DEBUG("after wd_HolePositioning\n");

    LOG_INFO("wd_HolePositioning: found %zu holes, err=%d runtime=%.3fms\n",
        holePositioning.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());

    // 记录统计信息到检测结果
    detectionResult.totalCandidates = static_cast<int>(holePositioning.size());
    detectionResult.filteredCount = 0;

    if (errCode != 0) {
        LOG_ERROR("wd_HolePositioning failed with error code: %d\n", errCode);
        return errCode;
    }

    // ========== 构建手眼标定齐次变换矩阵 ==========

    // T_end_to_cam（从4x4 clibMatrix数组）
    CTHomogeneousMatrix handEyeHM;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            handEyeHM.at(i, j) = clibMatrix[i * 4 + j];
        }
    }

    // 构建机器人法兰位姿齐次变换矩阵 T_base_to_end
    CTRobotPose ctRobotPose = CTRobotPose::fromDegrees(
        robotPose.x, robotPose.y, robotPose.z,
        robotPose.roll, robotPose.pitch, robotPose.yaw);
    CTHomogeneousMatrix T_robot = ctRobotPose.toHomogeneousMatrix();

    // 眼在手上：T_total = T_base_to_end * T_end_to_cam
    CTHomogeneousMatrix T_total = T_robot * handEyeHM;

    // 构建用于可视化的孔洞中心点列表
    std::vector<std::pair<SVzNL3DPointF, float>> holeVisData;  // center + radius

    // ========== 处理每个检测到的孔洞 ==========
    for (size_t i = 0; i < holePositioning.size(); i++) {
        const WD_HolePositionInfo& holeInfo = holePositioning[i];

        // 保存孔洞详细信息（center和normDir来自算法结果）
        HoleResultData holeData;
        holeData.centerX = static_cast<float>(holeInfo.center.x);
        holeData.centerY = static_cast<float>(holeInfo.center.y);
        holeData.centerZ = static_cast<float>(holeInfo.center.z);
        holeData.normalX = static_cast<float>(holeInfo.normDir.x);
        holeData.normalY = static_cast<float>(holeInfo.normDir.y);
        holeData.normalZ = static_cast<float>(holeInfo.normDir.z);
        // wd_HolePositioning 不输出 radius/depth/eccentricity 等，设为0
        holeData.radius = 0.0f;
        holeData.depth = 0.0f;
        holeData.eccentricity = 0.0f;
        holeData.radiusVariance = 0.0f;
        holeData.angularSpan = 0.0f;
        holeData.qualityScore = 0.0f;
        detectionResult.holeDetails.push_back(holeData);

        // ========== 六轴转换：位置+姿态（转换到机械臂坐标系） ==========

        // 1. 位置转换：使用齐次变换矩阵进行坐标变换
        CTVec3D ptEye(holeInfo.center.x, holeInfo.center.y, holeInfo.center.z);
        CTVec3D ptRobot = T_total.transformPoint(ptEye);

        // 2. 姿态转换：从法向量构建旋转矩阵
        // Z轴 = 法向量（归一化）
        CTVec3D Z_axis(holeInfo.normDir.x, holeInfo.normDir.y, holeInfo.normDir.z);
        double normZ = Z_axis.norm();
        if (normZ > 1e-6) {
            Z_axis = Z_axis * (1.0 / normZ);
        }

        // 选择 X 轴：使用上方向 (0,1,0) 叉乘 Z_axis
        CTVec3D up(0.0, 1.0, 0.0);
        CTVec3D X_axis = crossProduct(up, Z_axis);
        double normX = X_axis.norm();

        // 如果 up 和 Z_axis 平行，使用 (1,0,0) 作为备选
        if (normX < 1e-6) {
            up = CTVec3D(1.0, 0.0, 0.0);
            X_axis = crossProduct(up, Z_axis);
            normX = X_axis.norm();
        }

        if (normX > 1e-6) {
            X_axis = X_axis * (1.0 / normX);
        }

        // Y轴 = Z x X
        CTVec3D Y_axis = crossProduct(Z_axis, X_axis);

        // 构建方向向量列表（eye坐标系）
        CTVec3D dirVectors_eye[3];
        dirVectors_eye[0] = X_axis;
        dirVectors_eye[1] = Y_axis;
        dirVectors_eye[2] = Z_axis;

        // 根据配置决定方向向量反向
        switch (dirVectorInvert) {
            case DIR_INVERT_NONE:
                break;
            case DIR_INVERT_XY:
                dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
                dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
                break;
            case DIR_INVERT_XZ:
                dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
                dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
                break;
            case DIR_INVERT_YZ:
            default:
                dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
                dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
                break;
        }

        // 使用齐次变换矩阵对方向向量进行旋转变换（仅旋转，不平移）
        CTVec3D dirVectors_robot[3];
        for (int j = 0; j < 3; j++) {
            dirVectors_robot[j] = T_total.transformVector(dirVectors_eye[j]);
        }

        // 构建旋转矩阵
        CTRotationMatrix R_pose;
        R_pose.at(0, 0) = dirVectors_robot[0].x;
        R_pose.at(0, 1) = dirVectors_robot[1].x;
        R_pose.at(0, 2) = dirVectors_robot[2].x;
        R_pose.at(1, 0) = dirVectors_robot[0].y;
        R_pose.at(1, 1) = dirVectors_robot[1].y;
        R_pose.at(1, 2) = dirVectors_robot[2].y;
        R_pose.at(2, 0) = dirVectors_robot[0].z;
        R_pose.at(2, 1) = dirVectors_robot[1].z;
        R_pose.at(2, 2) = dirVectors_robot[2].z;

        // 使用 CCoordinateTransform::rotationMatrixToEuler 转换为欧拉角（外旋ZYX，即RZ-RY-RX）
        CTEulerAngles robotRpyRad = CCoordinateTransform::rotationMatrixToEuler(R_pose, CTEulerOrder::sZYX);
        double roll_deg, pitch_deg, yaw_deg;
        robotRpyRad.toDegrees(roll_deg, pitch_deg, yaw_deg);

        // 将机器人坐标系下的位姿添加到positions列表
        HolePosition centerRobotPos;
        centerRobotPos.x = ptRobot.x;
        centerRobotPos.y = ptRobot.y;
        centerRobotPos.z = ptRobot.z;
        centerRobotPos.roll  = roll_deg;
        centerRobotPos.pitch = pitch_deg;
        centerRobotPos.yaw   = yaw_deg;

        detectionResult.positions.push_back(centerRobotPos);

        // 保存可视化数据（半径设为0，因为 wd_HolePositioning 不输出半径）
        SVzNL3DPointF visCenter;
        visCenter.x = static_cast<float>(holeInfo.center.x);
        visCenter.y = static_cast<float>(holeInfo.center.y);
        visCenter.z = static_cast<float>(holeInfo.center.z);
        holeVisData.push_back(std::make_pair(visCenter, 0.0f));

        if (debugParam.enableDebug && debugParam.printDetailLog) {
            LOG_INFO("[Algo Thread] Hole[%zu] Normal (eye): (%.3f, %.3f, %.3f)\n",
                    i, holeInfo.normDir.x, holeInfo.normDir.y, holeInfo.normDir.z);
            LOG_INFO("[Algo Thread] Hole[%zu] Direction vectors (robot): X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n",
                    i,
                    dirVectors_robot[0].x, dirVectors_robot[0].y, dirVectors_robot[0].z,
                    dirVectors_robot[1].x, dirVectors_robot[1].y, dirVectors_robot[1].z,
                    dirVectors_robot[2].x, dirVectors_robot[2].y, dirVectors_robot[2].z);
            LOG_INFO("[Algo Thread] Hole[%zu] Center Eye   Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
                    i, holeInfo.center.x, holeInfo.center.y, holeInfo.center.z);
            LOG_INFO("[Algo Thread] Hole[%zu] Center Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, R=%.4f, P=%.4f, Y=%.4f\n",
                    i, ptRobot.x, ptRobot.y, ptRobot.z,
                    centerRobotPos.roll, centerRobotPos.pitch, centerRobotPos.yaw);
        }
    }

    // ========== 生成可视化图像 ==========
    {
        std::vector<HoleMarkerInfo> holes;
        for (const auto& vis : holeVisData) {
            HoleMarkerInfo h;
            h.center.x = vis.first.x;
            h.center.y = vis.first.y;
            h.center.z = vis.first.z;
            h.radius = vis.second;  // 半径为0
            holes.push_back(h);
        }
        detectionResult.image = PointCloudImageUtils::GenerateHoleDetectionImage(
            xyzData, holes,
            0.0,   // 绕X轴旋转角度（度），可按需调整视图
            0.0);  // 绕Y轴旋转角度（度），可按需调整视图
    }

    if (debugParam.enableDebug && debugParam.saveDebugImage) {
        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;
}