#include "DetectPresenter.h"
#include "AlgorithmParamConverter.h"
#include "CoordinateTransform.h"
#include "ScrewPositionTCPProtocol.h"
#include "rodAndBarDetection_Export.h"
#include <QColor>
#include <array>
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace {

constexpr double kVectorEpsilon = 1e-8;
constexpr double kRotationEpsilon = 1e-9;

QImage BuildScrewPointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
                                 const std::vector<SSX_rodPoseInfo>& screwInfo,
                                 const std::vector<std::pair<double, double>>* approachXY = nullptr)
{
    PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
    if (!canvas.isValid()) {
        return QImage();
    }

    constexpr double kAxisLineLength = 60.0;
    const QColor pointColor(0, 255, 0);
    const QColor lineColor(255, 0, 0);
    const QColor textColor(255, 255, 0);
    const QColor approachColor(255, 128, 0);  // 橙色：接近点

    for (size_t i = 0; i < screwInfo.size(); ++i) {
        const auto& screw = screwInfo[i];
        canvas.drawPoint(screw.center.x, screw.center.y, pointColor, 8);
        canvas.drawText(screw.center.x, screw.center.y, QString::number(i + 1), textColor, 16, 10, -10);
        canvas.drawLine(screw.center.x - kAxisLineLength * screw.axialDir.x,
                        screw.center.y - kAxisLineLength * screw.axialDir.y,
                        screw.center.x + kAxisLineLength * screw.axialDir.x,
                        screw.center.y + kAxisLineLength * screw.axialDir.y,
                        lineColor, 2);

        if (approachXY && i < approachXY->size()) {
            const auto& ap = (*approachXY)[i];
            canvas.drawPoint(ap.first, ap.second, approachColor, 6);
            canvas.drawLine(screw.center.x, screw.center.y, ap.first, ap.second, approachColor, 1);
        }
    }

    return canvas.image().copy();
}

QImage BuildToolDiskPointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& xyzData,
                                    const SSX_platePoseInfo& poseInfo,
                                    bool hasResult,
                                    const std::pair<double, double>* approachXY = nullptr)
{
    PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
    if (!canvas.isValid()) {
        return QImage();
    }

    if (hasResult) {
        constexpr double kNormalLineLength = 60.0;
        const QColor centerColor(0, 255, 0);
        const QColor normalColor(0, 180, 255);   // 蓝色：法向 Z
        const QColor xAxisColor(255, 0, 0);      // 红色：X 轴
        const QColor yAxisColor(0, 255, 0);      // 绿色：Y 轴
        const QColor textColor(255, 255, 0);
        const QColor approachColor(255, 128, 0); // 橙色：接近点

        // 绘制定位盘中心点
        canvas.drawPoint(poseInfo.center.x, poseInfo.center.y, centerColor, 10);

        // 绘制法向量方向线
        const double normalEndX = poseInfo.center.x + kNormalLineLength * poseInfo.normalDir.x;
        const double normalEndY = poseInfo.center.y + kNormalLineLength * poseInfo.normalDir.y;
        canvas.drawLine(poseInfo.center.x, poseInfo.center.y, normalEndX, normalEndY, normalColor, 2);
        canvas.drawText(normalEndX, normalEndY, QStringLiteral("Z"), normalColor, 14, 6, -6);

        const double xEndX = poseInfo.center.x + kNormalLineLength * poseInfo.xDir.x;
        const double xEndY = poseInfo.center.y + kNormalLineLength * poseInfo.xDir.y;
        canvas.drawLine(poseInfo.center.x, poseInfo.center.y, xEndX, xEndY, xAxisColor, 2);
        canvas.drawText(xEndX, xEndY, QStringLiteral("X"), xAxisColor, 14, 6, -6);

        const double yEndX = poseInfo.center.x + kNormalLineLength * poseInfo.yDir.x;
        const double yEndY = poseInfo.center.y + kNormalLineLength * poseInfo.yDir.y;
        canvas.drawLine(poseInfo.center.x, poseInfo.center.y, yEndX, yEndY, yAxisColor, 2);
        canvas.drawText(yEndX, yEndY, QStringLiteral("Y"), yAxisColor, 14, 6, -6);

        if (approachXY) {
            canvas.drawPoint(approachXY->first, approachXY->second, approachColor, 8);
            canvas.drawLine(poseInfo.center.x, poseInfo.center.y, approachXY->first, approachXY->second, approachColor, 1);
        }
    }

    return canvas.image().copy();
}

void SaveDebugImageIfNeeded(int cameraIndex,
                            const VrDebugParam& debugParam,
                            const QImage& image,
                            const QString& prefix)
{
    if (!debugParam.enableDebug || !debugParam.saveDebugImage || image.isNull()) {
        return;
    }

    const std::string timeStamp = CVrDateUtils::GetNowTime();
    const std::string fileName = debugParam.debugOutputPath + "/" +
                                 prefix.toStdString() + "_" + std::to_string(cameraIndex) + "_" + timeStamp + ".png";
    LOG_INFO("[Algo Thread] Debug image saved image : %s\n", fileName.c_str());
    image.save(QString::fromStdString(fileName));
}

CTHomogeneousMatrix BuildHandEyeMatrix(const double clibMatrix[16])
{
    CTHomogeneousMatrix handEyeMatrix;
    for (int row = 0; row < 4; ++row) {
        for (int col = 0; col < 4; ++col) {
            handEyeMatrix.at(row, col) = clibMatrix[row * 4 + col];
        }
    }
    return handEyeMatrix;
}

void ResolveRobotPoseAnglesDegrees(const RobotPose6D& robotPose,
                                   int poseOutputOrder,
                                   double& rxDeg,
                                   double& ryDeg,
                                   double& rzDeg)
{
    switch (poseOutputOrder) {
    case 1:
        rxDeg = robotPose.a;
        ryDeg = robotPose.b;
        rzDeg = robotPose.c;
        break;
    case 2:
        rxDeg = robotPose.b;
        ryDeg = robotPose.a;
        rzDeg = robotPose.c;
        break;
    case 3:
        rxDeg = robotPose.c;
        ryDeg = robotPose.a;
        rzDeg = robotPose.b;
        break;
    case 4:
        rxDeg = robotPose.b;
        ryDeg = robotPose.c;
        rzDeg = robotPose.a;
        break;
    case 5:
        rxDeg = robotPose.c;
        ryDeg = robotPose.b;
        rzDeg = robotPose.a;
        break;
    case 0:
    default:
        rxDeg = robotPose.a;
        ryDeg = robotPose.b;
        rzDeg = robotPose.c;
        break;
    }
}

// 将角度归一化到 (-180°, 180°]
double WrapDegreesTo180(double deg)
{
    while (deg > 180.0) {
        deg -= 360.0;
    }
    while (deg <= -180.0) {
        deg += 360.0;
    }
    return deg;
}

// 对 Tait-Bryan 欧拉角做等价归一化，使 pitch ∈ [-90°, 90°]
// 依据：(α, β, γ) ≡ (α±180°, 180°-β, γ±180°) 以及 (α±180°, -180°-β, γ±180°)
// 适用于 CTEulerOrder 中所有 12 种 Tait-Bryan 顺序（内旋/外旋一致）
void NormalizePitchRange(double& rollDeg, double& pitchDeg, double& yawDeg)
{
    pitchDeg = WrapDegreesTo180(pitchDeg);
    if (pitchDeg > 90.0) {
        pitchDeg = 180.0 - pitchDeg;
        rollDeg += 180.0;
        yawDeg += 180.0;
    } else if (pitchDeg < -90.0) {
        pitchDeg = -180.0 - pitchDeg;
        rollDeg += 180.0;
        yawDeg += 180.0;
    }
    rollDeg = WrapDegreesTo180(rollDeg);
    yawDeg = WrapDegreesTo180(yawDeg);
}

// 统一走 CCoordinateTransform::rotationMatrixToEuler（与 eulerToRotationMatrix 成对互逆），
// 再把 pitch 压入 [-90°, 90°]，保证对机器人侧输出的姿态语义稳定。
void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation,
                                            CTEulerOrder order,
                                            double& rollDeg,
                                            double& pitchDeg,
                                            double& yawDeg)
{
    const CTEulerAngles euler = CCoordinateTransform::rotationMatrixToEuler(rotation, order);
    euler.toDegrees(rollDeg, pitchDeg, yawDeg);
    NormalizePitchRange(rollDeg, pitchDeg, yawDeg);
}

// 万向锁消歧：当 |pitch| 接近 90° 时，Rz(yaw)·Ry(pitch)·Rx(roll) 的分解退化成一个自由度。
// 严格展开可得：
//   pitch = +90°  →  R 只依赖 (roll - yaw)；任意满足 roll - yaw = const 的组合都表示同一 R
//   pitch = -90°  →  R 只依赖 (roll + yaw)；任意满足 roll + yaw = const 的组合都表示同一 R
// 本函数把 yaw 锚定到 refYaw 附近，把剩余自由度全部放到 roll，
// 让不同帧的输出稳定落在参考（通常是机器人法兰的 rx、rz）附近。
//
// 【强消歧配置】：阈值覆盖 80°-85°，优先保证数值稳定性，代价是 pitch∈[80°,85°]
// 的过渡带会改变真实旋转矩阵（X 轴可能偏 3°-5°）。如需精确定姿请收窄阈值或换欧拉顺序。
//   |pitch| < kSoftThreshold：完全不改（保留真实分解）
//   |pitch| ≥ kHardThreshold：完全锁到参考（对 R 影响最小）
//   kSoftThreshold ≤ |pitch| < kHardThreshold：线性插值过渡
void ResolveGimbalAmbiguity(double& rollDeg, double pitchDeg, double& yawDeg,
                            double refRollDeg, double refYawDeg)
{
    constexpr double kSoftThresholdDeg = 80.0;
    constexpr double kHardThresholdDeg = 85.0;
    const double absPitch = std::abs(pitchDeg);
    if (absPitch < kSoftThresholdDeg) {
        return;
    }

    // 不变量方向与 pitch 符号的关系（由 Rz·Ry·Rx 展开式导出）：
    //   pitch > 0 → inv = roll - yaw
    //   pitch < 0 → inv = roll + yaw
    const bool positivePitch = (pitchDeg >= 0.0);
    const double sum    = positivePitch ? (rollDeg - yawDeg)       : (rollDeg + yawDeg);
    const double refSum = positivePitch ? (refRollDeg - refYawDeg) : (refRollDeg + refYawDeg);

    double adjustedSum = sum;
    while (adjustedSum - refSum >  180.0) { adjustedSum -= 360.0; }
    while (adjustedSum - refSum < -180.0) { adjustedSum += 360.0; }

    const double fullYaw  = WrapDegreesTo180(refYawDeg);
    // 反解 roll：positive 时 roll = sum + yaw；negative 时 roll = sum - yaw（与 sum 方向匹配）
    const double fullRoll = WrapDegreesTo180(positivePitch ? (adjustedSum + fullYaw)
                                                            : (adjustedSum - fullYaw));

    if (absPitch >= kHardThresholdDeg) {
        rollDeg = fullRoll;
        yawDeg  = fullYaw;
        return;
    }

    // 过渡带：alpha 从 0（kSoftThreshold）线性增到 1（kHardThreshold）
    const double alpha = (absPitch - kSoftThresholdDeg) /
                         (kHardThresholdDeg - kSoftThresholdDeg);
    auto lerpAngle = [alpha](double from, double to) {
        const double diff = WrapDegreesTo180(to - from);
        return WrapDegreesTo180(from + alpha * diff);
    };
    rollDeg = lerpAngle(rollDeg, fullRoll);
    yawDeg  = lerpAngle(yawDeg,  fullYaw);
}

CTVec3D NormalizeVector(const CTVec3D& vector)
{
    const double length = vector.norm();
    if (length < kVectorEpsilon) {
        return CTVec3D();
    }
    return vector * (1.0 / length);
}

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

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

bool IsValidVector(const CTVec3D& vector)
{
    return vector.norm() >= kVectorEpsilon;
}

CTVec3D ToCTVec3D(const SVzNL3DPoint& point)
{
    return CTVec3D(point.x, point.y, point.z);
}

bool TryProjectedAxis(const CTVec3D& reference, const CTVec3D& primary, CTVec3D& axis)
{
    const CTVec3D candidate = NormalizeVector(reference - primary * DotProduct(reference, primary));
    if (!IsValidVector(candidate)) {
        return false;
    }
    axis = candidate;
    return true;
}

bool BuildFrameFromXAxis(const CTVec3D& primary, std::array<CTVec3D, 3>& axes)
{
    const CTVec3D xAxis = NormalizeVector(primary);
    if (!IsValidVector(xAxis)) {
        return false;
    }

    CTVec3D yAxis = NormalizeVector(CrossProduct(CTVec3D(0.0, 0.0, 1.0), xAxis));
    if (!IsValidVector(yAxis) &&
        !TryProjectedAxis(CTVec3D(0.0, 1.0, 0.0), xAxis, yAxis) &&
        !TryProjectedAxis(CTVec3D(1.0, 0.0, 0.0), xAxis, yAxis)) {
        return false;
    }

    const CTVec3D zAxis = NormalizeVector(CrossProduct(xAxis, yAxis));
    if (!IsValidVector(zAxis)) {
        return false;
    }

    axes = {xAxis, yAxis, zAxis};
    return true;
}

void ApplyAxesRotation(std::array<CTVec3D, 3>& axes,
                       double rotXDeg, double rotYDeg, double rotZDeg)
{
    if (std::fabs(rotXDeg) < kRotationEpsilon &&
        std::fabs(rotYDeg) < kRotationEpsilon &&
        std::fabs(rotZDeg) < kRotationEpsilon) {
        return;
    }

    const double rx = rotXDeg * M_PI / 180.0;
    const double ry = rotYDeg * M_PI / 180.0;
    const double rz = rotZDeg * M_PI / 180.0;

    const double cx = std::cos(rx), sx = std::sin(rx);
    const double cy = std::cos(ry), sy = std::sin(ry);
    const double cz = std::cos(rz), sz = std::sin(rz);

    // R = Rx(rx) * Ry(ry) * Rz(rz)；对 Eye 坐标系下的工具轴组合（列向量）右乘 R，
    // 等价于在工具坐标系内按内旋 XYZ 顺序施加补偿旋转。
    double R[3][3];
    R[0][0] = cy * cz;
    R[0][1] = -cy * sz;
    R[0][2] = sy;
    R[1][0] = sx * sy * cz + cx * sz;
    R[1][1] = -sx * sy * sz + cx * cz;
    R[1][2] = -sx * cy;
    R[2][0] = -cx * sy * cz + sx * sz;
    R[2][1] = cx * sy * sz + sx * cz;
    R[2][2] = cx * cy;

    const CTVec3D oldX = axes[0];
    const CTVec3D oldY = axes[1];
    const CTVec3D oldZ = axes[2];

    axes[0] = oldX * R[0][0] + oldY * R[1][0] + oldZ * R[2][0];
    axes[1] = oldX * R[0][1] + oldY * R[1][1] + oldZ * R[2][1];
    axes[2] = oldX * R[0][2] + oldY * R[1][2] + oldZ * R[2][2];
}

bool TransformAxes(const std::array<CTVec3D, 3>& eyeAxes,
                   const CTHomogeneousMatrix& transform,
                   std::array<CTVec3D, 3>& robotAxes)
{
    for (size_t i = 0; i < eyeAxes.size(); ++i) {
        robotAxes[i] = NormalizeVector(transform.transformVector(eyeAxes[i]));
        if (!IsValidVector(robotAxes[i])) {
            return false;
        }
    }
    return true;
}

CTRotationMatrix BuildRotationMatrix(const std::array<CTVec3D, 3>& axes)
{
    CTRotationMatrix rotation;
    rotation.at(0, 0) = axes[0].x;
    rotation.at(0, 1) = axes[1].x;
    rotation.at(0, 2) = axes[2].x;
    rotation.at(1, 0) = axes[0].y;
    rotation.at(1, 1) = axes[1].y;
    rotation.at(1, 2) = axes[2].y;
    rotation.at(2, 0) = axes[0].z;
    rotation.at(2, 1) = axes[1].z;
    rotation.at(2, 2) = axes[2].z;
    return rotation;
}

}

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

DetectPresenter::~DetectPresenter()
{
}

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

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

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

    const ScrewDetectAlgorithmParams algoParams = AlgorithmParamConverter::ToScrewDetectAlgorithmParams(algorithmParams);
    const double rodDiameter = algoParams.rodDiameter;
    const bool isHorizonScan = algoParams.isHorizonScan;
    const SSG_cornerParam& cornerParam = algoParams.cornerParam;
    const SSG_treeGrowParam& growParam = algoParams.growParam;
    const SSG_outlierFilterParam& filterParam = algoParams.filterParam;

    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] Screw: rodDiameter=%.1f, isHorizonScan=%s\n", rodDiameter, isHorizonScan ? "true" : "false");
        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);
        LOG_INFO("[Algo Thread] Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f\n",
                 extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ);
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before sx_hexHeadScrewMeasure \n");

    std::vector<SSX_rodPoseInfo> screwInfo;
    sx_hexHeadScrewMeasure(
        xyzData,
        isHorizonScan,
        cornerParam,
        filterParam,
        growParam,
        rodDiameter,
        screwInfo,
        &errCode);

    LOG_DEBUG("after sx_hexHeadScrewMeasure \n");
    LOG_INFO("sx_hexHeadScrewMeasure: detected %zu screws, err=%d runtime=%.3fms\n",
             screwInfo.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    detectionResult.success = true;
    detectionResult.errorCode = 0;
    detectionResult.message = QStringLiteral("\u68c0\u6d4b\u6210\u529f");
    const CTHomogeneousMatrix handEyeMatrix = BuildHandEyeMatrix(clibMatrix);
    const CTEulerOrder order = static_cast<CTEulerOrder>(extrinsic.eulerOrder);

    double rx, ry, rz;
    ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz);
    const CTRobotPose flangePose = CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, rx, ry, rz);
    // flangePose.rx/ry/rz 已经是真实轴角，可直接交给 sixAxisEyeInHandBuildTransform 按 eulerOrder 组装。
    const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform(
            flangePose, order, handEyeMatrix);

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] Robot flange pose fields: X=%.3f, Y=%.3f, Z=%.3f, RX=%.3f, RY=%.3f, RZ=%.3f (eulerOrder=%d)\n",
                 robotPose.x, robotPose.y, robotPose.z, rx, ry, rz, static_cast<int>(order));
    }

    std::vector<std::pair<double, double>> approachEyeXY;
    approachEyeXY.reserve(screwInfo.size());

    for (size_t i = 0; i < screwInfo.size(); ++i) {
        const auto& screw = screwInfo[i];

        const CTVec3D eyeCenter = ToCTVec3D(screw.center);
        const CTVec3D eyeAxialDir = NormalizeVector(ToCTVec3D(screw.axialDir));
        const CTVec3D robotCenter = eyeInHandTransform.transformPoint(eyeCenter);
        const CTVec3D robotAxialDir = NormalizeVector(eyeInHandTransform.transformVector(eyeAxialDir));

        // 接近点：Eye 系沿螺杆轴偏移 offset，再经 T 变换到 Robot 系；姿态与目标点共用。
        const double offset = extrinsic.approachOffset;
        const CTVec3D eyeApproach(eyeCenter.x + eyeAxialDir.x * offset,
                                  eyeCenter.y + eyeAxialDir.y * offset,
                                  eyeCenter.z + eyeAxialDir.z * offset);
        const CTVec3D robotApproach = eyeInHandTransform.transformPoint(eyeApproach);
        approachEyeXY.emplace_back(eyeApproach.x, eyeApproach.y);

        // 姿态：以螺杆轴为 X 轴构建正交三元组 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。
        std::array<CTVec3D, 3> eyeAxes;
        if (!BuildFrameFromXAxis(eyeAxialDir, eyeAxes)) {
            LOG_WARNING("[Algo Thread] Screw %zu: 无法由轴向构建坐标系，已跳过\n", i);
            continue;
        }
        if (debugParam.enableDebug && debugParam.printDetailLog) {
            const CTRotationMatrix eyeRotationBefore = BuildRotationMatrix(eyeAxes);
            double eyeRollBefore = 0.0;
            double eyePitchBefore = 0.0;
            double eyeYawBefore = 0.0;
            RotationMatrixToConfiguredEulerDegrees(eyeRotationBefore, order, eyeRollBefore, eyePitchBefore, eyeYawBefore);
            LOG_INFO("[Algo Thread] Screw %zu Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
                     i, static_cast<int>(order), eyeRollBefore, eyePitchBefore, eyeYawBefore);
        }
        ApplyAxesRotation(eyeAxes, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ);
        if (debugParam.enableDebug && debugParam.printDetailLog) {
            const CTRotationMatrix eyeRotationAfter = BuildRotationMatrix(eyeAxes);
            double eyeRollAfter = 0.0;
            double eyePitchAfter = 0.0;
            double eyeYawAfter = 0.0;
            RotationMatrixToConfiguredEulerDegrees(eyeRotationAfter, order, eyeRollAfter, eyePitchAfter, eyeYawAfter);
            LOG_INFO("[Algo Thread] Screw %zu Eye Euler after compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
                     i, static_cast<int>(order), eyeRollAfter, eyePitchAfter, eyeYawAfter);
        }
        std::array<CTVec3D, 3> robotAxes;
        if (!TransformAxes(eyeAxes, eyeInHandTransform, robotAxes)) {
            LOG_WARNING("[Algo Thread] Screw %zu: 轴变换到机器人坐标系失败，已跳过\n", i);
            continue;
        }
        const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes);

        ScrewPosition pos;
        pos.x = robotCenter.x;
        pos.y = robotCenter.y;
        pos.z = robotCenter.z;
        pos.approachX = robotApproach.x;
        pos.approachY = robotApproach.y;
        pos.approachZ = robotApproach.z;
        RotationMatrixToConfiguredEulerDegrees(rotation, order, pos.roll, pos.pitch, pos.yaw);
        // 拍照姿态作参考，把 gimbal lock / 多解区的解拉回拍照姿态附近，减少机械臂旋转量。
        ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz);
        detectionResult.positions.push_back(pos);

        ScrewInfo info;
        info.centerX = robotCenter.x;
        info.centerY = robotCenter.y;
        info.centerZ = robotCenter.z;
        info.axialDirX = robotAxialDir.x;
        info.axialDirY = robotAxialDir.y;
        info.axialDirZ = robotAxialDir.z;
        info.rotateAngle = pos.roll;
        detectionResult.screwInfoList.push_back(info);

        if (debugParam.enableDebug && debugParam.printDetailLog) {
            LOG_INFO("[Algo Thread] Screw %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", i, screw.center.x, screw.center.y, screw.center.z);
            LOG_INFO("[Algo Thread] Screw %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, RPY=%.2f/%.2f/%.2f\n", i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
            LOG_INFO("[Algo Thread] Screw %zu Axial Dir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", i, screw.axialDir.x, screw.axialDir.y, screw.axialDir.z);
            LOG_INFO("[Algo Thread] Screw %zu Axial Dir Robot: X=%.3f, Y=%.3f, Z=%.3f\n", i, robotAxialDir.x, robotAxialDir.y, robotAxialDir.z);
            LOG_INFO("[Algo Thread] Screw %zu Approach (offset=%.2f): X=%.2f, Y=%.2f, Z=%.2f\n", i, offset, pos.approachX, pos.approachY, pos.approachZ);
        }
    }

    detectionResult.image = BuildScrewPointCloudImage(xyzData, screwInfo, &approachEyeXY);

    SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("Image"));
    return SUCCESS;
}

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

    std::vector<std::vector<SVzNL3DPosition>> xyzData;
    int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, xyzData);
    if (convertResult != SUCCESS || xyzData.empty()) {
        LOG_WARNING("Failed to convert tool disk data to XYZ format or no XYZ data available\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }

    // 构造算法参数（与螺杆检测共享 cornerParam）
    const ScrewDetectAlgorithmParams algoParams = AlgorithmParamConverter::ToScrewDetectAlgorithmParams(algorithmParams);
    const SSG_cornerParam& cornerParam = algoParams.cornerParam;

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Algo Thread] ToolDisk 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] ToolDisk Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f\n",
                 cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap, cornerParam.minEndingGap_z);
        LOG_INFO("[Algo Thread] ToolDisk Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f\n",
                 extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ);
    }

    int errCode = 0;
    CVrTimeUtils oTimeUtils;

    LOG_DEBUG("before sx_getLocationPlatePose\n");

    SSX_platePoseInfo poseInfo = sx_getLocationPlatePose(xyzData, cornerParam, &errCode);

    LOG_DEBUG("after sx_getLocationPlatePose\n");
    LOG_INFO("sx_getLocationPlatePose: err=%d runtime=%.3fms\n", errCode, oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(errCode);

    detectionResult.success = true;
    detectionResult.errorCode = 0;
    detectionResult.message = QStringLiteral("工具盘检测成功");
    const CTHomogeneousMatrix handEyeMatrix = BuildHandEyeMatrix(clibMatrix);
    const CTEulerOrder order = static_cast<CTEulerOrder>(extrinsic.eulerOrder);

    double rx, ry, rz;
    ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz);
    const CTRobotPose flangePose = CTRobotPose::fromDegrees( robotPose.x, robotPose.y, robotPose.z, rx, ry, rz);
    // flangePose.rx/ry/rz 已经是真实轴角，可直接交给 sixAxisEyeInHandBuildTransform 按 eulerOrder 组装。
    const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform(
            flangePose, order, handEyeMatrix);

    // 将定位盘中心点通过手眼标定转换为机器人坐标
    const CTVec3D robotCenter = eyeInHandTransform.transformPoint(ToCTVec3D(poseInfo.center));
    const CTVec3D eyeXAxis = NormalizeVector(ToCTVec3D(poseInfo.xDir));
    const CTVec3D eyeYAxis = NormalizeVector(ToCTVec3D(poseInfo.yDir));
    const CTVec3D eyeNormalDir = NormalizeVector(ToCTVec3D(poseInfo.normalDir));

    ScrewPosition pos;
    pos.x = robotCenter.x;
    pos.y = robotCenter.y;
    pos.z = robotCenter.z;
    // 接近点：Eye 系沿工具盘 X 轴偏移 offset，再经 T 变换到 Robot 系；姿态与目标点共用。
    const double offset = extrinsic.approachOffset;
    const CTVec3D eyeCenter = ToCTVec3D(poseInfo.center);
    const CTVec3D eyeApproach(eyeCenter.x + eyeXAxis.x * offset, eyeCenter.y + eyeXAxis.y * offset, eyeCenter.z + eyeXAxis.z * offset);
    const CTVec3D robotApproach = eyeInHandTransform.transformPoint(eyeApproach);
    pos.approachX = robotApproach.x;
    pos.approachY = robotApproach.y;
    pos.approachZ = robotApproach.z;
    const std::pair<double, double> approachEyeXY(eyeApproach.x, eyeApproach.y);

    // 姿态：三轴直接取算法输出 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。
    std::array<CTVec3D, 3> eyeAxes = {eyeXAxis, eyeYAxis, eyeNormalDir};
    if (debugParam.enableDebug && debugParam.printDetailLog) {
        const CTRotationMatrix eyeRotationBefore = BuildRotationMatrix(eyeAxes);
        double eyeRollBefore = 0.0;
        double eyePitchBefore = 0.0;
        double eyeYawBefore = 0.0;
        RotationMatrixToConfiguredEulerDegrees(eyeRotationBefore, order, eyeRollBefore, eyePitchBefore, eyeYawBefore);
        LOG_INFO("[Algo Thread] ToolDisk Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
                 static_cast<int>(order), eyeRollBefore, eyePitchBefore, eyeYawBefore);
    }
    ApplyAxesRotation(eyeAxes, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ);
    std::array<CTVec3D, 3> robotAxes;
    if (!TransformAxes(eyeAxes, eyeInHandTransform, robotAxes)) {
        LOG_WARNING("[Algo Thread] ToolDisk: 轴变换到机器人坐标系失败\n");
        return ERR_CODE(DEV_DATA_INVALID);
    }
    const CTRotationMatrix robotRotation = BuildRotationMatrix(robotAxes);
    RotationMatrixToConfiguredEulerDegrees(robotRotation, order, pos.roll, pos.pitch, pos.yaw);
    // 拍照姿态作参考，把 gimbal lock / 多解区的解拉回拍照姿态附近，减少机械臂旋转量。
    ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz);
    detectionResult.positions.push_back(pos);

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        const CTRotationMatrix eyeRotation = BuildRotationMatrix(eyeAxes);
        double eyeRoll = 0.0;
        double eyePitch = 0.0;
        double eyeYaw = 0.0;
        RotationMatrixToConfiguredEulerDegrees(eyeRotation, order, eyeRoll, eyePitch, eyeYaw);

        LOG_INFO("[Algo Thread] === Tool Disk Pose Debug ===\n");
        LOG_INFO("[Algo Thread] Eye Rotation Matrix:\n");
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", eyeRotation.at(0, 0), eyeRotation.at(0, 1), eyeRotation.at(0, 2));
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", eyeRotation.at(1, 0), eyeRotation.at(1, 1), eyeRotation.at(1, 2));
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", eyeRotation.at(2, 0), eyeRotation.at(2, 1), eyeRotation.at(2, 2));
        LOG_INFO("[Algo Thread] Eye Pos: (%.6f, %.6f, %.6f)\n",  poseInfo.center.x, poseInfo.center.y, poseInfo.center.z);
        LOG_INFO("[Algo Thread] Eye Euler after compensation (order=%d ): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
                 static_cast<int>(order), eyeRoll, eyePitch, eyeYaw);

        LOG_INFO("[Algo Thread] Robot Rotation Matrix:\n");
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", robotRotation.at(0, 0), robotRotation.at(0, 1), robotRotation.at(0, 2));
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", robotRotation.at(1, 0), robotRotation.at(1, 1), robotRotation.at(1, 2));
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", robotRotation.at(2, 0), robotRotation.at(2, 1), robotRotation.at(2, 2));
        LOG_INFO("[Algo Thread] Robot Pos: (%.6f, %.6f, %.6f)\n",  robotCenter.x, robotCenter.y, robotCenter.z);
        LOG_INFO("[Algo Thread] Robot Pos Euler (order=%d ): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
                 static_cast<int>(order), pos.roll, pos.pitch, pos.yaw);
        LOG_INFO("[Algo Thread] ToolDisk Approach (offset=%.2f): X=%.2f, Y=%.2f, Z=%.2f\n",
                 offset, pos.approachX, pos.approachY, pos.approachZ);
    }

    detectionResult.image = BuildToolDiskPointCloudImage(xyzData, poseInfo, true, &approachEyeXY);

    SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("ToolDisk_Image"));
    return SUCCESS;
}