#include "DetectPresenter.h"
#include "IHandEyeCalib.h"
#include "SG_baseAlgo_Export.h"
#include "SG_errCode.h"

#include <cmath>
#include <algorithm>
#include <cstring>
#include <exception>
#include <memory>

namespace {

const int kSupportedWorkpieceClassCount = 3;
const double kDuplicate2DIouThreshold = 0.5;

const char* WorkpieceModelLogName()
{
    return "WorkpiceTaperedModel";
}

char WorkpieceClassName(int classId)
{
    if (classId >= 0 && classId < 26) {
        return static_cast<char>('a' + classId);
    }
    return '?';
}

double RectArea(const WorkpiecePositionModelAdapter::Rect& rect)
{
    const double width = rect.right - rect.left;
    const double height = rect.bottom - rect.top;
    if (width <= 0.0 || height <= 0.0) {
        return 0.0;
    }
    return width * height;
}

double RectIou(const WorkpiecePositionModelAdapter::Rect& lhs,
               const WorkpiecePositionModelAdapter::Rect& rhs)
{
    const double interLeft = std::max(lhs.left, rhs.left);
    const double interTop = std::max(lhs.top, rhs.top);
    const double interRight = std::min(lhs.right, rhs.right);
    const double interBottom = std::min(lhs.bottom, rhs.bottom);
    WorkpiecePositionModelAdapter::Rect intersection;
    intersection.left = interLeft;
    intersection.top = interTop;
    intersection.right = interRight;
    intersection.bottom = interBottom;
    const double interArea = RectArea(intersection);
    if (interArea <= 0.0) {
        return 0.0;
    }

    const double unionArea = RectArea(lhs) + RectArea(rhs) - interArea;
    if (unionArea <= 0.0) {
        return 0.0;
    }
    return interArea / unionArea;
}

void FilterDuplicate2DDetections(
    std::vector<WorkpiecePositionModelAdapter::Detection>& detections)
{
    if (detections.size() < 2) {
        return;
    }

    std::stable_sort(detections.begin(), detections.end(),
        [](const WorkpiecePositionModelAdapter::Detection& lhs,
           const WorkpiecePositionModelAdapter::Detection& rhs) {
            return lhs.confidence > rhs.confidence;
        });

    std::vector<WorkpiecePositionModelAdapter::Detection> filtered;
    filtered.reserve(detections.size());
    for (const auto& detection : detections) {
        bool duplicated = false;
        for (const auto& kept : filtered) {
            if (RectIou(detection.objectRoi, kept.objectRoi) >= kDuplicate2DIouThreshold) {
                duplicated = true;
                break;
            }
        }
        if (!duplicated) {
            filtered.push_back(detection);
        }
    }

    detections.swap(filtered);
}

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 tool euler order %d, fallback to 11 (ZYX)\n", eulerOrder);
        return HECEulerOrder::ZYX;
    }
}

void ApplyToolRotationToEyeAxes(IHandEyeCalib& handEyeCalib,
                                HECEulerOrder eulerOrder,
                                const VrToolParam& toolParam,
                                std::vector<HECPoint3D>& axes)
{
    if (axes.size() != 3 ||
        (std::fabs(toolParam.rotX) < 1e-9 &&
         std::fabs(toolParam.rotY) < 1e-9 &&
         std::fabs(toolParam.rotZ) < 1e-9)) {
        return;
    }

    HECRotationMatrix toolRotation;
    handEyeCalib.EulerToRotationMatrix(
        HECEulerAngles::fromDegrees(toolParam.rotX, toolParam.rotY, toolParam.rotZ),
        eulerOrder,
        toolRotation);

    const std::vector<HECPoint3D> originalAxes = axes;
    for (int column = 0; column < 3; ++column) {
        axes[column] = (originalAxes[0] * toolRotation.at(0, column)
                      + originalAxes[1] * toolRotation.at(1, column)
                      + originalAxes[2] * toolRotation.at(2, column)).normalized();
    }
}

void ApplyConfiguredAxisInversion(int dirVectorInvert, std::vector<HECPoint3D>& axes)
{
    if (axes.size() != 3) {
        return;
    }

    switch (dirVectorInvert) {
    case DIR_INVERT_XY:
        axes[0] = axes[0] * -1.0;
        axes[1] = axes[1] * -1.0;
        break;
    case DIR_INVERT_XZ:
        axes[0] = axes[0] * -1.0;
        axes[2] = axes[2] * -1.0;
        break;
    case DIR_INVERT_YZ:
        axes[1] = axes[1] * -1.0;
        axes[2] = axes[2] * -1.0;
        break;
    case DIR_INVERT_NONE:
    default:
        break;
    }
}

SSG_planeCalibPara ToGroundCalibParam(const VrCameraPlaneCalibParam* cameraCalibParam)
{
    SSG_planeCalibPara groundCalibPara;
    const double identity[9] = {1.0, 0.0, 0.0,
                                0.0, 1.0, 0.0,
                                0.0, 0.0, 1.0};
    if (cameraCalibParam) {
        std::memcpy(groundCalibPara.planeCalib,
                    cameraCalibParam->planeCalib,
                    sizeof(groundCalibPara.planeCalib));
        std::memcpy(groundCalibPara.invRMatrix,
                    cameraCalibParam->invRMatrix,
                    sizeof(groundCalibPara.invRMatrix));
        groundCalibPara.planeHeight = cameraCalibParam->planeHeight;
    } else {
        std::memcpy(groundCalibPara.planeCalib, identity, sizeof(identity));
        std::memcpy(groundCalibPara.invRMatrix, identity, sizeof(identity));
        groundCalibPara.planeHeight = -1.0;
    }
    return groundCalibPara;
}

WorkpiecePoint3D ToResultPoint(const HECPoint3D& point)
{
    return WorkpiecePoint3D(point.x, point.y, point.z);
}

HECPoint3D ToHandEyePoint(const SVzNL3DPoint& point)
{
    return HECPoint3D(point.x, point.y, point.z);
}

SVzNL3DPoint ToVzPoint(const double values[3])
{
    SVzNL3DPoint point;
    point.x = values[0];
    point.y = values[1];
    point.z = values[2];
    return point;
}

WD_HRM_BinInfo ToWdBinInfo(const VrBinParam& param)
{
    WD_HRM_BinInfo binInfo;
    std::memset(&binInfo, 0, sizeof(binInfo));
    binInfo.length = param.length;
    binInfo.width = param.width;
    binInfo.binTopZ = param.binTopZ;
    binInfo.center = ToVzPoint(param.center);
    binInfo.bottomNormal = ToVzPoint(param.bottomNormal);
    binInfo.x_dir = ToVzPoint(param.xDir);
    binInfo.y_dir = ToVzPoint(param.yDir);
    for (int i = 0; i < 4; ++i) {
        binInfo.minRectVertex[i] = ToVzPoint(param.minRectVertex[i]);
    }
    return binInfo;
}

void LogTaperedAlgorithmInput(
    const std::vector<std::vector<SVzNLPositionD>>& scanLines,
    const std::vector<WD_objArea2D>& objROIs,
    const SSG_planeCalibPara& groundCalibPara,
    const VrAlgorithmParams& algorithmParams,
    const double clibMatrix[16],
    int eulerOrder,
    int dirVectorInvert)
{
    LOG_INFO("[Tapered Debug] input summary: lines=%zu\n", scanLines.size());

    LOG_INFO("[Tapered Debug] params: eulerOrder=%d, dirVectorInvert=%d, "
             "toolEulerOrder=%d, toolRot=(%.6f,%.6f,%.6f), "
             "toolOffset=(%.6f,%.6f,%.6f) \n",
             eulerOrder, dirVectorInvert,
             algorithmParams.toolParam.eulerOrder,
             algorithmParams.toolParam.rotX,
             algorithmParams.toolParam.rotY,
             algorithmParams.toolParam.rotZ,
             algorithmParams.toolParam.offsetX,
             algorithmParams.toolParam.offsetY,
             algorithmParams.toolParam.offsetZ);

    LOG_INFO("[Tapered Debug] planeCalib: planeHeight=%.6f\n", groundCalibPara.planeHeight);
    LOG_INFO("[Tapered Debug] planeCalib=[%.9f %.9f %.9f; %.9f %.9f %.9f; %.9f %.9f %.9f]\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("[Tapered Debug] invRMatrix=[%.9f %.9f %.9f; %.9f %.9f %.9f; %.9f %.9f %.9f]\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]);
    LOG_INFO("[Tapered Debug] handEye=[%.9f %.9f %.9f %.9f; %.9f %.9f %.9f %.9f; "
             "%.9f %.9f %.9f %.9f; %.9f %.9f %.9f %.9f]\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("[Tapered Debug] objROIs=%zu\n", objROIs.size());
    for (size_t index = 0; index < objROIs.size(); ++index) {
        const WD_objArea2D& roi = objROIs[index];
        LOG_INFO("[Tapered Debug] objROI[%zu]: type=%d, score=%.6f, LTRB=(%.3f,%.3f,%.3f,%.3f)\n",
                 index, roi.workpieceType,  roi.score2D,
                 roi.roi.left, roi.roi.top, roi.roi.right, roi.roi.bottom);
    }
}

void LogRawTaperedOutputs(const std::vector<WD_workpieceInfo>& workpiecePositions)
{
    for (size_t index = 0; index < workpiecePositions.size(); ++index) {
        const WD_workpieceInfo& workpiece = workpiecePositions[index];
        LOG_INFO("[Tapered Debug] rawOutput[%zu]: id=%d, type=%d, value=%.9f, "
                 "center=(%.9f,%.9f,%.9f), "
                 "x_dir=(%.9f,%.9f,%.9f), y_dir=(%.9f,%.9f,%.9f), z_dir=(%.9f,%.9f,%.9f)\n",
                 index, workpiece.id, workpiece.workpieceType, workpiece.value,
                 workpiece.center.x, workpiece.center.y, workpiece.center.z,
                 workpiece.x_dir.x, workpiece.x_dir.y, workpiece.x_dir.z,
                 workpiece.y_dir.x, workpiece.y_dir.y, workpiece.y_dir.z,
                 workpiece.z_dir.x, workpiece.z_dir.y, workpiece.z_dir.z);
    }

}

} // namespace

DetectPresenter::DetectPresenter()
{
    const char* version = wd_hybridPositioningVersion();
    LOG_INFO("DetectPresenter Init hybrid positioning algorithm ver: %s\n",
             version ? version : "unknown");
}

DetectPresenter::~DetectPresenter()
{
}

QString DetectPresenter::GetAlgoVersion()
{
    const char* version = wd_hybridPositioningVersion();
    return version ? QString::fromLocal8Bit(version) : QStringLiteral("未知");
}

int DetectPresenter::LocateWorkpiece2D(
    const SVzNLImageData& leftImage,
    const VrWorkpieceModelParam& modelParam,
    const VrDebugParam& debugParam,
    std::vector<WorkpiecePositionModelAdapter::Detection>& detections)
{
    CVrTimeUtils modelTimer;
    const WorkpiecePositionModelAdapter::Status modelStatus =
        m_workpieceModel.Locate(leftImage, modelParam.minimumScore, detections);
    LOG_INFO("%s: found %zu ROIs, status=%d runtime=%.3fms\n",
             WorkpieceModelLogName(), detections.size(),
             static_cast<int>(modelStatus), modelTimer.GetElapsedTimeInMilliSec());
    if (modelStatus != WorkpiecePositionModelAdapter::Status::Ok) {
        const QString errorMessage = m_workpieceModel.LastError();
        LOG_ERROR("%s failed: %s\n",
                  WorkpieceModelLogName(), errorMessage.toStdString().c_str());
        switch (modelStatus) {
        case WorkpiecePositionModelAdapter::Status::RuntimeUnavailable:
            return ERR_CODE(FUN_UNSUPPORT);
        case WorkpiecePositionModelAdapter::Status::InferenceFailed:
            return ERR_CODE(APP_ERR_EXEC);
        case WorkpiecePositionModelAdapter::Status::OutOfMemory:
            return ERR_CODE(DATA_ERR_MEM);
        case WorkpiecePositionModelAdapter::Status::InvalidResult:
            return ERR_CODE(DATA_ERR_INVALID);
        case WorkpiecePositionModelAdapter::Status::InvalidArgument:
        case WorkpiecePositionModelAdapter::Status::UnsupportedImageFormat:
        default:
            return ERR_CODE(DEV_DATA_INVALID);
        }
    }
    if (detections.empty()) {
        LOG_WARNING("%s returned no workpiece ROI\n", WorkpieceModelLogName());
        return ERR_CODE(SX_ERR_ZERO_2D_OBJECTS);
    }

    const size_t rawDetectionCount = detections.size();
    FilterDuplicate2DDetections(detections);
    if (detections.size() != rawDetectionCount) {
        LOG_INFO("%s duplicate ROI filter: raw=%zu, filtered=%zu, iouThreshold=%.2f\n",
                 WorkpieceModelLogName(), rawDetectionCount, detections.size(),
                 kDuplicate2DIouThreshold);
    }

    LOG_INFO("%s 2D model output results:\n", WorkpieceModelLogName());
    for (size_t index = 0; index < detections.size(); ++index) {
        const WorkpiecePositionModelAdapter::Detection& detection = detections[index];
        const WorkpiecePositionModelAdapter::Rect& roi = detection.objectRoi;
        LOG_INFO("obj_%zu: C=%c, Rect={ %.3f, %.3f, %.3f, %.3f }, Score=%.4f\n",
                 index + 1,
                 WorkpieceClassName(detection.classId),
                 roi.left,
                 roi.top,
                 roi.right - roi.left,
                 roi.bottom - roi.top,
                 detection.confidence);
    }

    if (debugParam.enableDebug && debugParam.printDetailLog) {
        for (size_t index = 0; index < detections.size(); ++index) {
            const WorkpiecePositionModelAdapter::Detection& detection = detections[index];
            LOG_INFO("[2D ROI %zu] class=%d "
                     "label=%s score=%.3f "
                     "objectLTRB=(%.1f,%.1f,%.1f,%.1f) mask=%s\n",
                     index, detection.classId, detection.label.toStdString().c_str(), detection.confidence,
                     detection.objectRoi.left, detection.objectRoi.top,
                     detection.objectRoi.right, detection.objectRoi.bottom,
                     detection.HasMask() ? "yes" : "no");
        }
    }

    return SUCCESS;
}

int DetectPresenter::DetectStatorWorkpiecePosition(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const std::vector<WorkpiecePositionModelAdapter::Detection>& detections,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    const double clibMatrix[16],
    int eulerOrder,
    int dirVectorInvert,
    const WD_HRM_BinInfo* binInfo,
    StatorWorkpiecePositionDetectionResult& detectionResult)
{
    detectionResult.cameraIndex = cameraIndex;
    detectionResult.errorCode = SUCCESS;
    detectionResult.positions.clear();

    if (laserLines.empty()) {
        detectionResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
        return detectionResult.errorCode;
    }
    if (detections.empty()) {
        detectionResult.errorCode = ERR_CODE(SX_ERR_ZERO_2D_OBJECTS);
        return detectionResult.errorCode;
    }

    std::vector<std::vector<SVzNLPositionD>> algorithmData;
    const int positionResult = dataLoader.ConvertToSVzNLPositionD(
        laserLines, algorithmData);
    if (positionResult != SUCCESS) {
        detectionResult.errorCode = positionResult;
        LOG_WARNING("Failed to convert VzNLSDK data to SVzNLPositionD, err=%d, detail=%s\n",
                    positionResult, dataLoader.GetLastError().c_str());
        return positionResult;
    }

    if (algorithmData.empty()) {
        detectionResult.errorCode = ERR_CODE(DEV_DATA_INVALID);
        LOG_WARNING("Tapered algorithm input has no valid scan lines\n");
        return detectionResult.errorCode;
    }
    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LOG_INFO("[Tapered Debug] SVzNLPositionD input lines: %zu\n",
                 algorithmData.size());
    }

    VrCameraPlaneCalibParam cameraCalibParamValue;
    const VrCameraPlaneCalibParam* cameraCalibParam = nullptr;
    if (algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraCalibParamValue) &&
        cameraCalibParamValue.isCalibrated) {
        cameraCalibParam = &cameraCalibParamValue;
    }
    const SSG_planeCalibPara groundCalibPara = ToGroundCalibParam(cameraCalibParam);

    std::vector<WD_objArea2D> objROIs;
    objROIs.reserve(detections.size());
    std::vector<int> sourceDetectionIndices;
    sourceDetectionIndices.reserve(detections.size());
    for (size_t index = 0; index < detections.size(); ++index) {
        const WorkpiecePositionModelAdapter::Detection& detection = detections[index];
        if (detection.classId < 0 || detection.classId >= kSupportedWorkpieceClassCount) {
            LOG_WARNING("Ignoring unsupported 2D class at ROI %zu: class=%d\n", index, detection.classId);
            continue;
        }

        const int workpieceType = algorithmParams.modelParam.WorkpieceTypeForClass(detection.classId);
        if (workpieceType <= 0) {
            LOG_INFO("Ignoring disabled 2D class at ROI %zu: class=%d, configured workpieceType=%d\n",
                     index, detection.classId, workpieceType);
            continue;
        }   

        const WorkpiecePositionModelAdapter::Rect& roi = detection.objectRoi;
        if (!std::isfinite(detection.confidence) ||
            detection.confidence < 0.0 || detection.confidence > 1.0 ||
            !std::isfinite(roi.left) || !std::isfinite(roi.top) ||
            !std::isfinite(roi.right) || !std::isfinite(roi.bottom) ||
            roi.right <= roi.left || roi.bottom <= roi.top) {
            detectionResult.errorCode = ERR_CODE(DATA_ERR_INVALID);
            LOG_ERROR("Invalid 2D ROI at index %zu: class=%d score=%.6f "
                      "LTRB=(%.3f,%.3f,%.3f,%.3f)\n",
                      index,detection.classId, detection.confidence,
                      roi.left, roi.top, roi.right, roi.bottom);
            return detectionResult.errorCode;
        }

        WD_objArea2D objROI = {};
        objROI.workpieceType = workpieceType; // class0/1/2 -> a/b/c -> 1/2/3
        objROI.roi.left = roi.left;
        objROI.roi.top = roi.top;
        objROI.roi.right = roi.right;
        objROI.roi.bottom = roi.bottom;
        objROI.score2D = detection.confidence;
        objROIs.push_back(objROI);
        sourceDetectionIndices.push_back(static_cast<int>(index));
    }

    if (objROIs.empty()) {
        detectionResult.errorCode = ERR_CODE(SX_ERR_ZERO_2D_OBJECTS);
        LOG_WARNING("No enabled 2D ROI is available for tapered workpiece positioning\n");
        return detectionResult.errorCode;
    }

    std::vector<WD_workpieceInfo> workpiecePositions;
    CVrTimeUtils timer;
    int algorithmResult = SUCCESS;
    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LogTaperedAlgorithmInput(algorithmData, objROIs, groundCalibPara, algorithmParams, clibMatrix, eulerOrder, dirVectorInvert);
    }
    (void)binInfo;
    size_t totalPoints = 0;
    for (const auto& line : algorithmData) {
        totalPoints += line.size();
    }
    LOG_INFO("[Tapered] entering wd_HRM_TaperedWorkpiecePositioning: lines=%zu points=%zu objROIs=%zu\n",
             algorithmData.size(), totalPoints, objROIs.size());
    try {
        wd_HRM_TaperedWorkpiecePositioning(algorithmData, objROIs, groundCalibPara, workpiecePositions, &algorithmResult);
        LOG_INFO("wd_HRM_TaperedWorkpiecePositioning returned: found %zu workpieces, err=%d runtime=%.3fms\n",
                 workpiecePositions.size(), algorithmResult, timer.GetElapsedTimeInMilliSec());
    } catch (const std::exception& e) {
        LOG_ERROR("wd_HRM_TaperedWorkpiecePositioning threw exception: %s\n", e.what());
        detectionResult.errorCode = ERR_CODE(APP_ERR_EXEC);
        return detectionResult.errorCode;
    } catch (...) {
        LOG_ERROR("wd_HRM_TaperedWorkpiecePositioning threw unknown exception\n");
        detectionResult.errorCode = ERR_CODE(APP_ERR_EXEC);
        return detectionResult.errorCode;
    }
    if (debugParam.enableDebug && debugParam.printDetailLog) {
        LogRawTaperedOutputs(workpiecePositions);
    }
    if (algorithmResult != SUCCESS) {
        detectionResult.errorCode = algorithmResult;
        return algorithmResult;
    }
    if (workpiecePositions.empty()) {
        detectionResult.errorCode = ERR_CODE(SX_ERR_ZERO_OBJECTS);
        LOG_WARNING("Tapered workpiece algorithm returned success without a target\n");
        return detectionResult.errorCode;
    }
    std::unique_ptr<IHandEyeCalib, decltype(&DestroyHandEyeCalibInstance)> handEyeCalib(
        CreateHandEyeCalibInstance(), DestroyHandEyeCalibInstance);
    if (!handEyeCalib) {
        detectionResult.errorCode = ERR_CODE(DEV_NOT_FIND);
        return detectionResult.errorCode;
    }

    (void)eulerOrder; // The supplied 4x4 matrix already fixes the hand-eye rotation.
    const HECCalibResult calibResult = HECCalibResult::fromHomogeneousArray(clibMatrix);
    const VrToolParam& toolParam = algorithmParams.toolParam;
    const HECEulerOrder toolEulerOrder = ToHandEyeEulerOrder(toolParam.eulerOrder);

    for (size_t index = 0; index < workpiecePositions.size(); ++index) {
        const WD_workpieceInfo& workpiece = workpiecePositions[index];
        WorkpiecePosition position;
        position.workpieceType = workpiece.workpieceType;
        if (workpiece.id > 0 && workpiece.id <= static_cast<int>(sourceDetectionIndices.size())) {
            position.sourceDetectionIndex = sourceDetectionIndices[workpiece.id - 1];
        } else if (index < sourceDetectionIndices.size()) {
            position.sourceDetectionIndex = sourceDetectionIndices[index];
            LOG_WARNING("Invalid workpiece id=%d at algorithm output %zu, fallback sourceDetectionIndex=%d\n",
                        workpiece.id, index, position.sourceDetectionIndex);
        } else {
            position.sourceDetectionIndex = static_cast<int>(index);
            LOG_WARNING("Invalid workpiece id=%d at algorithm output %zu, fallback output index=%d\n",
                        workpiece.id, index, position.sourceDetectionIndex);
        }
        position.value = workpiece.value;

        HECPoint3D robotCenter;
        handEyeCalib->TransformPoint(calibResult.R, calibResult.T, ToHandEyePoint(workpiece.center), robotCenter);
        position.center = ToResultPoint(robotCenter);
        position.x = robotCenter.x + toolParam.offsetX;
        position.y = robotCenter.y + toolParam.offsetY;
        position.z = robotCenter.z + toolParam.offsetZ;

        std::vector<HECPoint3D> eyeAxes;
        eyeAxes.push_back(ToHandEyePoint(workpiece.x_dir).normalized());
        eyeAxes.push_back(ToHandEyePoint(workpiece.y_dir).normalized());
        eyeAxes.push_back(ToHandEyePoint(workpiece.z_dir).normalized());
        ApplyConfiguredAxisInversion(dirVectorInvert, eyeAxes);
        ApplyToolRotationToEyeAxes(*handEyeCalib, toolEulerOrder, toolParam, eyeAxes);

        std::vector<HECPoint3D> robotAxes(3);
        for (int axis = 0; axis < 3; ++axis) {
            handEyeCalib->RotatePoint(calibResult.R, eyeAxes[axis], robotAxes[axis]);
            robotAxes[axis] = robotAxes[axis].normalized();
        }
        position.xDir = ToResultPoint(robotAxes[0]);
        position.yDir = ToResultPoint(robotAxes[1]);
        position.zDir = ToResultPoint(robotAxes[2]);

        double rotation[3][3] = {
            {robotAxes[0].x, robotAxes[1].x, robotAxes[2].x},
            {robotAxes[0].y, robotAxes[1].y, robotAxes[2].y},
            {robotAxes[0].z, robotAxes[1].z, robotAxes[2].z}
        };
        const SSG_EulerAngles robotRpy = rotationMatrixToEulerZYX(rotation);
        position.roll = robotRpy.roll;
        position.pitch = robotRpy.pitch;
        position.yaw = robotRpy.yaw;

        if (debugParam.enableDebug && debugParam.printDetailLog) {
            LOG_INFO("[Tapered Debug] finalOutput[%zu]: id=%d, sourceDetectionIndex=%d, type=%d, value=%.9f, "
                     "centerRobot=(%.9f,%.9f,%.9f), targetXYZ=(%.9f,%.9f,%.9f), "
                     "RPY=(%.9f,%.9f,%.9f)\n",
                     index,
                     workpiece.id,
                     position.sourceDetectionIndex,
                     position.workpieceType,
                     position.value,
                     position.center.x, position.center.y, position.center.z,
                     position.x, position.y, position.z,
                     position.roll, position.pitch, position.yaw);
        }

        detectionResult.positions.push_back(position);
    }

    return SUCCESS;
}

int DetectPresenter::DetectBin(
    int cameraIndex,
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
    const VrAlgorithmParams& algorithmParams,
    const VrDebugParam& debugParam,
    LaserDataLoader& dataLoader,
    WD_HRM_BinInfo& binInfo)
{
    (void)debugParam;
    if (laserLines.empty()) {
        return ERR_CODE(DEV_DATA_INVALID);
    }

    std::vector<std::vector<SVzNL3DPosition>> scanLines;
    const int convertResult = dataLoader.ConvertToSVzNL3DPosition(laserLines, scanLines);
    if (convertResult != SUCCESS) {
        LOG_WARNING("Failed to convert VzNLSDK data to SVzNL3DPosition for bin detection, err=%d, detail=%s\n",
                    convertResult, dataLoader.GetLastError().c_str());
        return convertResult;
    }
    if (scanLines.empty()) {
        return ERR_CODE(DEV_DATA_INVALID);
    }

    VrCameraPlaneCalibParam cameraCalibParamValue;
    const VrCameraPlaneCalibParam* cameraCalibParam = nullptr;
    if (algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraCalibParamValue) &&
        cameraCalibParamValue.isCalibrated) {
        cameraCalibParam = &cameraCalibParamValue;
    }
    const SSG_planeCalibPara groundCalibPara = ToGroundCalibParam(cameraCalibParam);

    CVrTimeUtils timer;
    int errCode = SUCCESS;
    size_t totalPoints = 0;
    for (const auto& line : scanLines) {
        totalPoints += line.size();
    }
    LOG_INFO("[Tapered] entering wd_HRM_getBinSize: lines=%zu points=%zu binHeight=%.3f\n",
             scanLines.size(), totalPoints, algorithmParams.binParam.binHeight);
    try {
        binInfo = wd_HRM_getBinSize(
            scanLines,
            groundCalibPara,
            algorithmParams.binParam.binHeight,
            &errCode);
        LOG_INFO("wd_HRM_getBinSize returned: err=%d runtime=%.3fms, binHeight=%.3f, length=%.3f, width=%.3f, topZ=%.3f, center=(%.3f,%.3f,%.3f)\n",
                 errCode, timer.GetElapsedTimeInMilliSec(),
                 algorithmParams.binParam.binHeight,
                 binInfo.length, binInfo.width, binInfo.binTopZ,
                 binInfo.center.x, binInfo.center.y, binInfo.center.z);
    } catch (const std::exception& e) {
        LOG_ERROR("wd_HRM_getBinSize threw exception: %s\n", e.what());
        return ERR_CODE(APP_ERR_EXEC);
    } catch (...) {
        LOG_ERROR("wd_HRM_getBinSize threw unknown exception\n");
        return ERR_CODE(APP_ERR_EXEC);
    }
    return errCode;
}