GrabBag/App/RodAndBarPosition/RodAndBarPositionApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"
#include "rodAndBarDetection_Export.h"
#include "AlgoParamConverter.h"
#include "IHandEyeCalib.h"
#include <cmath>
#include <fstream>
#include <memory>
#include <QBrush>
#include <QColor>
#include <QLineF>
#include <QPainter>
#include <QPolygonF>

namespace
{

HECEulerOrder ToHandEyeEulerOrder(int eulerOrder)
{
    switch (eulerOrder) {
    case 10: return HECEulerOrder::XYZ;
    case 11: return HECEulerOrder::ZYX;
    case 12: return HECEulerOrder::ZXY;
    case 13: return HECEulerOrder::YXZ;
    case 14: return HECEulerOrder::YZX;
    case 15: return HECEulerOrder::XZY;
    default:
        LOG_WARNING("Unsupported euler order %d, fallback to 11(sZYX)\n", eulerOrder);
        return HECEulerOrder::ZYX;
    }
}

double NormalizeAngleDeg(double angle)
{
    double normalized = std::fmod(angle + 180.0, 360.0);
    if (normalized < 0.0) {
        normalized += 360.0;
    }
    return normalized - 180.0;
}

bool IsFullAxialAngleRange(double rangeMin, double rangeMax)
{
    if (!std::isfinite(rangeMin) || !std::isfinite(rangeMax)) {
        return true;
    }
    return std::fabs(rangeMax - rangeMin) >= 359.999
        || (rangeMin <= -180.0 && rangeMax >= 180.0);
}

bool IsAngleInRange(double angle, double rangeMin, double rangeMax)
{
    if (IsFullAxialAngleRange(rangeMin, rangeMax)) {
        return true;
    }

    const double a = NormalizeAngleDeg(angle);
    const double minAngle = NormalizeAngleDeg(rangeMin);
    const double maxAngle = NormalizeAngleDeg(rangeMax);
    constexpr double kEpsilon = 1e-9;

    if (minAngle <= maxAngle) {
        return a + kEpsilon >= minAngle && a - kEpsilon <= maxAngle;
    }
    return a + kEpsilon >= minAngle || a - kEpsilon <= maxAngle;
}

struct AxialDirectionAdjustment
{
    HECPoint3D direction;
    double inputAngle = 0.0;
    double outputAngle = 0.0;
    bool angleValid = false;
    bool flipped = false;
};

AxialDirectionAdjustment AdjustAxialDirectionByRange(const HECPoint3D& axialDir,
                                                     double rangeMin,
                                                     double rangeMax)
{
    AxialDirectionAdjustment result;
    result.direction = axialDir;

    const double xyNorm = std::hypot(axialDir.x, axialDir.y);
    if (xyNorm < 1e-9 || IsFullAxialAngleRange(rangeMin, rangeMax)) {
        return result;
    }

    result.angleValid = true;
    result.inputAngle = NormalizeAngleDeg(std::atan2(axialDir.y, axialDir.x) * 180.0 / M_PI);
    result.outputAngle = result.inputAngle;

    if (IsAngleInRange(result.inputAngle, rangeMin, rangeMax)) {
        return result;
    }

    const double flippedAngle = NormalizeAngleDeg(result.inputAngle + 180.0);
    if (IsAngleInRange(flippedAngle, rangeMin, rangeMax)) {
        result.direction = axialDir * -1.0;
        result.outputAngle = flippedAngle;
        result.flipped = true;
    }

    return result;
}

double DotProduct(const HECPoint3D& a, const HECPoint3D& b)
{
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

HECPoint3D CrossProduct(const HECPoint3D& a, const HECPoint3D& b)
{
    return HECPoint3D(a.y * b.z - a.z * b.y,
                      a.z * b.x - a.x * b.z,
                      a.x * b.y - a.y * b.x);
}

bool BuildEyeAxes(const HECPoint3D& axialDir,
                  const HECPoint3D& normalDir,
                  HECPoint3D& xAxis,
                  HECPoint3D& yAxis,
                  HECPoint3D& zAxis)
{
    xAxis = axialDir.normalized();
    zAxis = normalDir.normalized();
    if (xAxis.norm() < 1e-6 || zAxis.norm() < 1e-6) {
        return false;
    }

    zAxis = (zAxis - xAxis * DotProduct(xAxis, zAxis)).normalized();
    if (zAxis.norm() < 1e-6) {
        return false;
    }

    yAxis = CrossProduct(zAxis, xAxis).normalized();
    if (yAxis.norm() < 1e-6) {
        return false;
    }

    zAxis = CrossProduct(xAxis, yAxis).normalized();
    return zAxis.norm() >= 1e-6;
}

void ApplyToolRotationToEyeAxes(IHandEyeCalib& handEyeCalib,
                                HECEulerOrder eulerOrder,
                                double toolRotX,
                                double toolRotY,
                                double toolRotZ,
                                HECPoint3D& xAxis,
                                HECPoint3D& yAxis,
                                HECPoint3D& zAxis)
{
    if (std::abs(toolRotX) < 1e-9 &&
        std::abs(toolRotY) < 1e-9 &&
        std::abs(toolRotZ) < 1e-9) {
        return;
    }

    HECRotationMatrix toolRotation;
    handEyeCalib.EulerToRotationMatrix(
        HECEulerAngles::fromDegrees(toolRotX, toolRotY, toolRotZ),
        eulerOrder,
        toolRotation);

    const HECPoint3D oldX = xAxis;
    const HECPoint3D oldY = yAxis;
    const HECPoint3D oldZ = zAxis;

    xAxis = (oldX * toolRotation.at(0, 0)
           + oldY * toolRotation.at(1, 0)
           + oldZ * toolRotation.at(2, 0)).normalized();
    yAxis = (oldX * toolRotation.at(0, 1)
           + oldY * toolRotation.at(1, 1)
           + oldZ * toolRotation.at(2, 1)).normalized();
    zAxis = (oldX * toolRotation.at(0, 2)
           + oldY * toolRotation.at(1, 2)
           + oldZ * toolRotation.at(2, 2)).normalized();
}

void DrawCanvasArrow(PointCloudCanvas& canvas,
                     double startX,
                     double startY,
                     double endX,
                     double endY,
                     const QColor& color,
                     int penWidth)
{
    if (!canvas.isValid()) {
        return;
    }

    QLineF line(QPointF(canvas.project(startX, startY)),
                QPointF(canvas.project(endX, endY)));
    if (line.length() < 1.0) {
        return;
    }

    QPainter painter(&canvas.image());
    painter.setRenderHint(QPainter::Antialiasing);
    painter.setPen(QPen(color, penWidth, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
    painter.setBrush(QBrush(color));
    painter.drawLine(line);

    const double arrowSize = 14.0;
    const double angle = std::atan2(line.dy(), line.dx());
    const QPointF arrowP1 = line.p2() - QPointF(std::cos(angle - M_PI / 6.0) * arrowSize,
                                                std::sin(angle - M_PI / 6.0) * arrowSize);
    const QPointF arrowP2 = line.p2() - QPointF(std::cos(angle + M_PI / 6.0) * arrowSize,
                                                std::sin(angle + M_PI / 6.0) * arrowSize);

    QPolygonF arrowHead;
    arrowHead << line.p2() << arrowP1 << arrowP2;
    painter.drawPolygon(arrowHead);
}

} // namespace

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

DetectPresenter::~DetectPresenter()
{
}

QString DetectPresenter::GetAlgoVersion()
{
    return QString(wd_rodAndBarDetectionVersion());
}

int DetectPresenter::DetectRod(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    int handEyeEulerOrder,
    double axialAngleMin,
    double axialAngleMax,
    double toolRotX,
    double toolRotY,
    double toolRotZ,
    double toolOffsetX,
    double toolOffsetY,
    double toolOffsetZ,
    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保存点云已由 BasePresenter::DetectTask() 统一处理，此处不再重复保存
    std::string timeStamp = CVrDateUtils::GetNowTime();

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

    // 使用 AlgoParamConverter 进行参数转换
    SSX_rodParam rodParam = AlgoParamConverter::ToAlgoParam(algorithmParams.rodParam);
    SSG_cornerParam cornerParam = AlgoParamConverter::ToAlgoParam(algorithmParams.cornerParam);
    SSG_treeGrowParam growParam = AlgoParamConverter::ToAlgoParam(algorithmParams.growParam);
    SSG_outlierFilterParam filterParam = AlgoParamConverter::ToAlgoParam(algorithmParams.filterParam);

    // 构建平面标定参数
    SSG_planeCalibPara poseCalibPara = AlgoParamConverter::ToAlgoPlaneCalibParam(cameraCalibParam);

    if(debugParam.enableDebug && debugParam.printDetailLog)
    {
        AlgoParamConverter::LogAlgoParams("[Algo Thread]", rodParam, cornerParam, filterParam, growParam, clibMatrix);
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before sx_rodPositioning \n");

    // 调用棒材定位算法
    std::vector<SSX_rodPositionInfo> rodInfo;
    sx_rodPositioning(
        xyzData,
        poseCalibPara,
        cornerParam,
        filterParam,
        growParam,
        rodParam,
        rodInfo,
        &errCode);

    LOG_DEBUG("after sx_rodPositioning \n");

    LOG_INFO("sx_rodPositioning: detected %zu rods, err=%d runtime=%.3fms\n", rodInfo.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);
    std::unique_ptr<IHandEyeCalib, decltype(&DestroyHandEyeCalibInstance)> handEyeCalib(
                                                CreateHandEyeCalibInstance(),
                                                DestroyHandEyeCalibInstance);
    if (!handEyeCalib) {
        LOG_ERROR("Failed to create HandEyeCalib instance\n");
        return ERR_CODE(DEV_NOT_FIND);
    }

    const HECCalibResult calibResult = HECCalibResult::fromHomogeneousArray(clibMatrix);
    // Tool Euler order belongs to the hand-eye conversion chain, not the protocol A/B/C field order.
    const HECEulerOrder hecEulerOrder = ToHandEyeEulerOrder(handEyeEulerOrder);

    std::vector<AxialDirectionAdjustment> axialAdjustments;
    axialAdjustments.reserve(rodInfo.size());
    for (const auto& rod : rodInfo) {
        axialAdjustments.push_back(AdjustAxialDirectionByRange(
            HECPoint3D(rod.axialDir.x, rod.axialDir.y, rod.axialDir.z),
            axialAngleMin,
            axialAngleMax));
    }

    // 使用 PointCloudCanvas 生成点云图像（灰色底图 + 棒材中心/方向线标记）
    {
        PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);

        for (size_t i = 0; i < rodInfo.size(); i++) {
            const auto& rod = rodInfo[i];
            const HECPoint3D& axialDir = axialAdjustments[i].direction;

            // 绘制棒材中心点（红色）
            canvas.drawPoint(rod.center.x, rod.center.y, QColor(255, 0, 0), 6);

            // 绘制轴向箭头（黄色）
            const double axialXYNorm = std::hypot(axialDir.x, axialDir.y);
            if (axialXYNorm > 1e-9) {
                const double axisUx = axialDir.x / axialXYNorm;
                const double axisUy = axialDir.y / axialXYNorm;
                const double axisHalfLen = std::max(60.0, algorithmParams.rodParam.rodLen * 0.25);
                const double ax0 = rod.center.x - axisHalfLen * axisUx;
                const double ay0 = rod.center.y - axisHalfLen * axisUy;
                const double ax1 = rod.center.x + axisHalfLen * axisUx;
                const double ay1 = rod.center.y + axisHalfLen * axisUy;
                DrawCanvasArrow(canvas, ax0, ay0, ax1, ay1, QColor(255, 220, 0), 3);
            }

            // 绘制起点到终点线段（绿色）
            canvas.drawLine(rod.startPt.x, rod.startPt.y, rod.endPt.x, rod.endPt.y, QColor(0, 255, 0), 2);

            // 中心点白色编号
            canvas.drawText(rod.center.x, rod.center.y, QString("%1").arg(i + 1), Qt::white, 14);
        }

        detectionResult.image = canvas.image();
    }

    // 转换检测结果为UI显示格式（使用机械臂坐标系数据）
    for (size_t i = 0; i < rodInfo.size(); i++) {
        const auto& rod = rodInfo[i];
        const AxialDirectionAdjustment& axialAdjustment = axialAdjustments[i];
        const HECPoint3D rawAxialDir(rod.axialDir.x, rod.axialDir.y, rod.axialDir.z);
        const HECPoint3D adjustedAxialDir = axialAdjustment.direction;
        const HECPoint3D normalDir(rod.normalDir.x, rod.normalDir.y, rod.normalDir.z);

        HECPoint3D poseAxialDir = adjustedAxialDir;
        HECPoint3D poseNormalDir = normalDir;
        HECPoint3D poseYAxis;
        if (BuildEyeAxes(adjustedAxialDir, normalDir, poseAxialDir, poseYAxis, poseNormalDir)) {
            ApplyToolRotationToEyeAxes(
                *handEyeCalib,
                hecEulerOrder,
                toolRotX,
                toolRotY,
                toolRotZ,
                poseAxialDir,
                poseYAxis,
                poseNormalDir);
        }
        
        LOG_INFO("[Algo Thread] Rod %zu Eye Center: X=%.2f, Y=%.2f, Z=%.2f\n", i, rod.center.x, rod.center.y, rod.center.z);
        LOG_INFO("[Algo Thread] Rod %zu Raw X seed: [%.6f, %.6f, %.6f]\n", i, rawAxialDir.x, rawAxialDir.y, rawAxialDir.z);
        if (axialAdjustment.angleValid) {
            LOG_INFO("[Algo Thread] Rod %zu Axial angle adjust: raw=%.3f, range=(%.3f, %.3f), output=%.3f, flipped=%d\n",
                     i,
                     axialAdjustment.inputAngle,
                     axialAngleMin,
                     axialAngleMax,
                     axialAdjustment.outputAngle,
                     axialAdjustment.flipped ? 1 : 0);
        }
        LOG_INFO("[Algo Thread] Rod %zu Input X seed: [%.6f, %.6f, %.6f]\n", i, poseAxialDir.x, poseAxialDir.y, poseAxialDir.z);
        LOG_INFO("[Algo Thread] Rod %zu Input Z seed: [%.6f, %.6f, %.6f]\n", i, poseNormalDir.x, poseNormalDir.y, poseNormalDir.z);

        HECPoseResult poseResult;
        bool validPose = handEyeCalib->TransformPose(
            calibResult,
            HECPoint3D(rod.center.x, rod.center.y, rod.center.z),
            poseAxialDir,
            poseNormalDir,
            0,
            hecEulerOrder,
            HECLongAxisDir::AxisX,
            poseResult);
        if (!validPose) {
            LOG_WARNING("[Algo Thread] Rod %zu has invalid axial/normal direction, use zero pose\n", i);
        }

        poseResult.position.x += toolOffsetX;
        poseResult.position.y += toolOffsetY;
        poseResult.position.z += toolOffsetZ;

        double rollDeg = 0.0, pitchDeg = 0.0, yawDeg = 0.0;
        poseResult.angles.toDegrees(rollDeg, pitchDeg, yawDeg);

        // 创建位置数据（使用转换后的机械臂坐标）
        RodPosition pos;
        pos.roll    = rollDeg;
        pos.pitch   = pitchDeg;
        pos.yaw     = yawDeg;
        pos.x       = poseResult.position.x;
        pos.y       = poseResult.position.y;
        pos.z       = poseResult.position.z;

        detectionResult.positions.push_back(pos);

        // 保存棒材信息
        RodInfo info;
        info.centerX = poseResult.position.x;
        info.centerY = poseResult.position.y;
        info.centerZ = poseResult.position.z;
        info.axialDirX = adjustedAxialDir.x;
        info.axialDirY = adjustedAxialDir.y;
        info.axialDirZ = adjustedAxialDir.z;
        info.normalDirX = rod.normalDir.x;
        info.normalDirY = rod.normalDir.y;
        info.normalDirZ = rod.normalDir.z;
        info.startPtX = rod.startPt.x;
        info.startPtY = rod.startPt.y;
        info.startPtZ = rod.startPt.z;
        info.endPtX = rod.endPt.x;
        info.endPtY = rod.endPt.y;
        info.endPtZ = rod.endPt.z;
        detectionResult.rodInfoList.push_back(info);

        // Print key values for coordinate transform debugging
        LOG_INFO("[Algo Thread] Rod %zu Robot Pose: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.6f, Pitch=%.6f, Yaw=%.6f\n", i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
    }

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