#include "DetectPresenter.h"

#include <algorithm>
#include <cmath>
#include <utility>
#include <vector>

#include "AapgsModelClassifier.h"
#include "IVrConfig.h"
#include "PersionModelDetector.h"
#include "SG_errCode.h"
#include "planeLocalization_Export.h"

namespace
{
struct PlaneLocalizationResult
{
    SSX_planeInfo info{};
    int errorCode = 0;
    QString errorMessage;
};

bool IsFinitePoint(const SVzNL3DPoint& point)
{
    return std::isfinite(point.x) &&
           std::isfinite(point.y) &&
           std::isfinite(point.z);
}

bool IsPersonDetection(const PersionModelDetector::Detection& detection)
{
    const QString label = detection.label.trimmed();
    return label.compare(QStringLiteral("person"), Qt::CaseInsensitive) == 0 ||
           (label.isEmpty() && detection.classId == 0);
}

bool ConvertCloudToScanLines(
    const RsCloudData& cloud,
    std::vector<std::vector<SVzNL3DPosition>>& scanLines,
    QString& errorMessage)
{
    scanLines.clear();
    errorMessage.clear();
    scanLines.reserve(cloud.size());

    size_t validPointCount = 0;
    for (const auto& item : cloud) {
        const EVzResultDataType dataType = item.first;
        const SVzLaserLineData& sourceLine = item.second;
        if (sourceLine.nPointCount < 0 ||
            (sourceLine.nPointCount > 0 && !sourceLine.p3DPoint)) {
            errorMessage = QStringLiteral("点云包含无效扫描线");
            return false;
        }
        if (dataType != keResultDataType_PointXYZI &&
            dataType != keResultDataType_Position) {
            errorMessage = QStringLiteral("停机算法不支持点云类型：%1")
                               .arg(static_cast<int>(dataType));
            return false;
        }

        std::vector<SVzNL3DPosition> targetLine;
        targetLine.resize(static_cast<size_t>(sourceLine.nPointCount));
        if (dataType == keResultDataType_PointXYZI) {
            const auto* points =
                static_cast<const SVzNLPointXYZI*>(sourceLine.p3DPoint);
            for (int pointIndex = 0;
                 pointIndex < sourceLine.nPointCount;
                 ++pointIndex) {
                SVzNL3DPosition& target =
                    targetLine[static_cast<size_t>(pointIndex)];
                target.nPointIdx = pointIndex;
                target.pt3D.x = static_cast<double>(points[pointIndex].x);
                target.pt3D.y = static_cast<double>(points[pointIndex].y);
                target.pt3D.z = static_cast<double>(points[pointIndex].z);
                if (!IsFinitePoint(target.pt3D)) {
                    target.pt3D = { 0.0, 0.0, 0.0 };
                } else if (std::fabs(target.pt3D.x) > 1e-6 ||
                           std::fabs(target.pt3D.y) > 1e-6 ||
                           std::fabs(target.pt3D.z) > 1e-6) {
                    ++validPointCount;
                }
            }
        } else {
            const auto* points =
                static_cast<const SVzNL3DPosition*>(sourceLine.p3DPoint);
            for (int pointIndex = 0;
                 pointIndex < sourceLine.nPointCount;
                 ++pointIndex) {
                targetLine[static_cast<size_t>(pointIndex)] = points[pointIndex];
                SVzNL3DPoint& point =
                    targetLine[static_cast<size_t>(pointIndex)].pt3D;
                if (!IsFinitePoint(point)) {
                    point = { 0.0, 0.0, 0.0 };
                } else if (std::fabs(point.x) > 1e-6 ||
                           std::fabs(point.y) > 1e-6 ||
                           std::fabs(point.z) > 1e-6) {
                    ++validPointCount;
                }
            }
        }
        scanLines.push_back(std::move(targetLine));
    }

    if (scanLines.empty() || validPointCount == 0) {
        errorMessage = QStringLiteral("点云不包含有效三维点");
        return false;
    }
    return true;
}

SSX_planeParkingParam BuildParkingParam(const VrPlaneParkingParam& source)
{
    SSX_planeParkingParam target{};
    target.parkingPoint = { source.parkingPointX,
                            source.parkingPointY,
                            source.parkingPointZ };
    target.guideLinePoint = { source.guideLinePointX,
                              source.guideLinePointY,
                              source.guideLinePointZ };
    target.guidingRange = source.guidingRange;
    target.parkingRange = source.parkingRange;
    target.distFromNoseToWheel = source.distFromNoseToWheel;
    return target;
}

SSG_planeCalibPara BuildGroundCalibrationParam(
    const VrPlaneGroundCalibrationParam& source)
{
    SSG_planeCalibPara target{};
    for (int i = 0; i < 9; ++i) {
        target.planeCalib[i] = source.planeCalib[i];
        target.invRMatrix[i] = source.invRMatrix[i];
    }
    target.planeHeight = source.planeHeight;
    return target;
}

SSG_treeGrowParam BuildTreeGrowParam(const VrPlaneTreeGrowParam& source)
{
    SSG_treeGrowParam target{};
    target.yDeviation_max = source.yDeviationMax;
    target.zDeviation_max = source.zDeviationMax;
    target.maxLineSkipNum = source.maxLineSkipNum;
    target.maxSkipDistance = source.maxSkipDistance;
    target.minLTypeTreeLen = source.minLTypeTreeLen;
    target.minVTypeTreeLen = source.minVTypeTreeLen;
    return target;
}

PlaneLocalizationResult LocalizePlane(const RsCloudData& cloud,
                                      const VrAlgorithmParams& algorithmParams)
{
    PlaneLocalizationResult result;
    std::vector<std::vector<SVzNL3DPosition>> scanLines;
    if (!ConvertCloudToScanLines(cloud, scanLines, result.errorMessage)) {
        result.errorCode = cloud.empty() ? SG_ERR_3D_DATA_NULL
                                         : SG_ERR_3D_DATA_INVLD;
        return result;
    }

    std::vector<std::vector<SVzNL3DPosition>> debugData;
    int algorithmError = 0;
    result.info = wd_planeLocalization(
        scanLines,
        BuildGroundCalibrationParam(algorithmParams.groundCalibrationParam),
        BuildParkingParam(algorithmParams.planeParkingParam),
        BuildTreeGrowParam(algorithmParams.treeGrowParam),
        debugData,
        &algorithmError);
    result.errorCode = algorithmError;

    if (algorithmError == SX_ERR_NO_PLANE_FOUND) {
        result.errorMessage = QStringLiteral("未找到有效飞机目标");
    } else if (algorithmError == SX_ERR_NOSEPOINT_FAIL) {
        result.errorMessage = QStringLiteral("飞机机鼻定位失败");
    } else if (algorithmError == SX_ERR_ENDINE_FAIL) {
        result.errorMessage = QStringLiteral("飞机主体特征提取失败");
    } else if (algorithmError != 0) {
        result.errorMessage = QStringLiteral("飞机定位算法错误：%1")
                                  .arg(algorithmError);
    } else if (!std::isfinite(result.info.distance) ||
               !std::isfinite(result.info.deviation) ||
               !std::isfinite(result.info.dirAngle_deg) ||
               !IsFinitePoint(result.info.nosePoint) ||
               !IsFinitePoint(result.info.axis)) {
        result.errorCode = SG_ERR_3D_DATA_INVLD;
        result.errorMessage = QStringLiteral("飞机定位算法返回非有限数值");
    }
    return result;
}

bool IsPlaneLostError(int errorCode)
{
    return errorCode == SX_ERR_NO_PLANE_FOUND ||
           errorCode == SX_ERR_NOSEPOINT_FAIL ||
           errorCode == SX_ERR_ENDINE_FAIL ||
           errorCode == SG_ERR_3D_DATA_NULL;
}

bool IsAlignmentGuidanceState(ParkingGuideState state)
{
    return state == ParkingGuideState::CenterLineAligned ||
           state == ParkingGuideState::AzimuthGuidance;
}

bool IsPastAlignmentGuidanceState(ParkingGuideState state)
{
    return state == ParkingGuideState::StopPositionReached ||
           state == ParkingGuideState::DockingCompleted ||
           state == ParkingGuideState::Overshot ||
           state == ParkingGuideState::StoppedShort ||
           state == ParkingGuideState::ChocksOn;
}

double IntegerMillimeters(double value)
{
    if (!std::isfinite(value)) {
        return 0.0;
    }
    return static_cast<double>(std::lround(value));
}

bool ShouldClearPublishedSpeed(int guideStateCode)
{
    switch (ParkingGuideStateFromCode(guideStateCode)) {
    case ParkingGuideState::StopPositionReached:
    case ParkingGuideState::DockingCompleted:
    case ParkingGuideState::ChocksOn:
        return true;
    default:
        return false;
    }
}

void ApplyPublishedGuideMetrics(ParkingSpaceGuideInfo& info)
{
    // 对外距离/横向偏移只报毫米整数；到位后速度按静止处理。
    info.distance = IntegerMillimeters(info.distance);
    info.lateralOffset = IntegerMillimeters(info.lateralOffset);
    if (ShouldClearPublishedSpeed(info.guideStateCode)) {
        info.aircraftSpeed = 0.0;
    }
}

bool ShouldFilterMeasurementJump(ParkingGuideState previousGuideState,
                                 double currentDistance,
                                 double stopTolerance,
                                 double approachStartDistance)
{
    // 上一帧已在对准中线/方位引导：即使本帧跳出该距离，仍按突变忽略误检。
    if (IsAlignmentGuidanceState(previousGuideState)) {
        return true;
    }

    // 到位、越位、提前停止后不再拦截，避免锁死最终状态。
    if (IsPastAlignmentGuidanceState(previousGuideState)) {
        return false;
    }

    // 捕获/开始引导阶段仅在本帧已进入对中距离时过滤，避免误检直接跳进该过程。
    return currentDistance >= 0.0 &&
           currentDistance <= approachStartDistance &&
           std::fabs(currentDistance) > stopTolerance;
}

QString GuideText(ParkingGuideState state)
{
    switch (state) {
    case ParkingGuideState::DockingStarted: return QStringLiteral("START");
    case ParkingGuideState::Capturing: return QStringLiteral("CAPTURE");
    case ParkingGuideState::Tracking: return QStringLiteral("TRACKING");
    case ParkingGuideState::ApproachRate: return QStringLiteral("APPROACH");
    case ParkingGuideState::CenterLineAligned: return QStringLiteral("CENTER");
    case ParkingGuideState::Slow: return QStringLiteral("SLOW");
    case ParkingGuideState::AzimuthGuidance: return QStringLiteral("AZIMUTH");
    case ParkingGuideState::StopPositionReached: return QStringLiteral("STOP");
    case ParkingGuideState::DockingCompleted: return QStringLiteral("OK");
    case ParkingGuideState::Overshot: return QStringLiteral("TOOFAR");
    case ParkingGuideState::StoppedShort: return QStringLiteral("STOP SHORT");
    case ParkingGuideState::SlowAircraftLost: return QStringLiteral("SLOW+LOST");
    case ParkingGuideState::AircraftVerificationFailed:
        return QStringLiteral("STOP+IDFAIL");
    case ParkingGuideState::TooFast: return QStringLiteral("STOPTOOFAST");
    case ParkingGuideState::EmergencyStop: return QStringLiteral("STOP");
    case ParkingGuideState::ChocksOn: return QStringLiteral("CHOCK ON");
    case ParkingGuideState::SystemError: return QStringLiteral("ERROR");
    default: return QStringLiteral("WAIT");
    }
}

void SetGuideState(ParkingSpaceGuideInfo& info, ParkingGuideState state)
{
    info.guideStateCode = static_cast<int>(state);
    info.guideText = GuideText(state);
}

ParkingSpaceGuideInfo MakeLastKnownInfo(
    const ParkingGuideAlgorithmState& state)
{
    ParkingSpaceGuideInfo info;
    const QString selectedModelType = state.selectedModelType.trimmed();
    const QString recognizedModelType = state.recognizedModelType.trimmed();
    info.expectedModelType = selectedModelType;
    info.modelType = recognizedModelType.isEmpty()
        ? QStringLiteral("未知")
        : recognizedModelType;
    info.modelMatched = state.modelVerified;
    info.modelVerificationSupported = state.modelVerificationSupported;
    info.personInChockRegion = state.personInChockRegion;
    info.chocksConfirmed = state.chocksConfirmed;
    info.personCount = state.personCount;
    info.personConfidence = state.personConfidence;
    info.distance = IntegerMillimeters(state.lastDistance);
    info.lateralOffset = IntegerMillimeters(state.lastDeviation);
    info.angle = state.lastAngle;
    info.aircraftSpeed = state.filteredApproachSpeed / 1000.0;
    if (ShouldClearPublishedSpeed(state.lastGuideStateCode)) {
        info.aircraftSpeed = 0.0;
    }
    info.confidence = std::isfinite(state.modelConfidence)
        ? state.modelConfidence
        : 0.0;
    return info;
}
}

DetectPresenter::DetectPresenter()
    : m_modelClassifier(std::make_unique<AapgsModelClassifier>())
    , m_personDetector(std::make_unique<PersionModelDetector>())
{
}

DetectPresenter::~DetectPresenter() = default;

bool ModelRecognitionResult::IsVerified(double minimumConfidence) const
{
    if (!std::isfinite(minimumConfidence)) {
        return false;
    }

    const QString normalizedModel = modelType.trimmed();
    const bool isUnknown = normalizedModel == QStringLiteral("未知") ||
        normalizedModel.compare(QStringLiteral("unknown"), Qt::CaseInsensitive) == 0;
    const double threshold = (std::max)(0.0, minimumConfidence);
    return verified &&
           !normalizedModel.isEmpty() &&
           !isUnknown &&
           std::isfinite(confidence) &&
           confidence >= threshold;
}

QString DetectPresenter::GetAlgoVersion()
{
    const char* planeVersion = wd_PlaneLocalizationVersion();
    return QStringLiteral(
        "planeLocalization %1 / AAPGS_model 20260804 / Persion_model 1.0.0")
        .arg(planeVersion ? QString::fromLocal8Bit(planeVersion)
                          : QStringLiteral("未知"));
}

int DetectPresenter::CalibrateGround(
    const RsCloudData& cloud,
    VrPlaneGroundCalibrationParam& calibration,
    QString& errorMessage)
{
    std::vector<std::vector<SVzNL3DPosition>> scanLines;
    if (!ConvertCloudToScanLines(cloud, scanLines, errorMessage)) {
        return -1;
    }

    const SSG_planeCalibPara algorithmCalibration =
        wd_getGroundCalibPara(scanLines);
    for (int i = 0; i < 9; ++i) {
        if (!std::isfinite(algorithmCalibration.planeCalib[i]) ||
            !std::isfinite(algorithmCalibration.invRMatrix[i])) {
            errorMessage = QStringLiteral("地面调平算法返回无效矩阵");
            return -1;
        }
        calibration.planeCalib[i] = algorithmCalibration.planeCalib[i];
        calibration.invRMatrix[i] = algorithmCalibration.invRMatrix[i];
    }
    if (!std::isfinite(algorithmCalibration.planeHeight)) {
        errorMessage = QStringLiteral("地面调平算法返回无效地面高度");
        return -1;
    }
    calibration.planeHeight = algorithmCalibration.planeHeight;
    errorMessage.clear();
    return 0;
}

int DetectPresenter::DetectAirplanePresence(const RsCloudData& cloud,
                                            const VrAlgorithmParams& algorithmParams,
                                            AirplanePresenceResult& result)
{
    result = AirplanePresenceResult();
    const PlaneLocalizationResult localization =
        LocalizePlane(cloud, algorithmParams);
    if (localization.errorCode == 0) {
        result.state = AirplanePresenceState::Detected;
        result.message = QStringLiteral("检测到飞机，距离停机点 %1 mm")
                             .arg(localization.info.distance, 0, 'f', 1);
        return 0;
    }

    if (IsPlaneLostError(localization.errorCode)) {
        result.state = AirplanePresenceState::NotDetected;
        result.message = localization.errorMessage.trimmed().isEmpty()
            ? QStringLiteral("未检测到飞机")
            : localization.errorMessage;
        return 0;
    }

    result.state = AirplanePresenceState::ViewBlocked;
    result.message = localization.errorMessage.trimmed().isEmpty()
        ? QStringLiteral("飞机检测视野或点云异常")
        : localization.errorMessage;
    return 0;
}

int DetectPresenter::DetectParkingSpaceGuide(const RsCloudData& cloud,
                                             const VrAlgorithmParams& algorithmParams,
                                             qint64 frameTimestampMs,
                                             const ParkingGuideAlgorithmState& previousState,
                                             DetectionResult& result,
                                             ParkingGuideAlgorithmControl& control)
{
    result = DetectionResult();
    control = ParkingGuideAlgorithmControl();
    control.nextState = previousState;
    result.cameraIndex = 1;
    result.errorCode = 0;

    const VrGuideDecisionParam& guideParam = algorithmParams.guideDecisionParam;
    const VrParkingProcessParam& processParam = algorithmParams.processParam;
    const VrModelRecognitionParam& modelParam =
        algorithmParams.modelRecognitionParam;
    PlaneLocalizationResult localization =
        LocalizePlane(cloud, algorithmParams);
    control.planeLocalizationErrorCode = localization.errorCode;
    control.planeLocalizationMessage = localization.errorMessage;

    if (localization.errorCode == 0) {
        // 按所选机型表中的补偿修正距离，后续判定/显示/协议都用补偿后值。
        localization.info.distance -= previousState.stopDistanceCompensation;
    }

    if (localization.errorCode != 0) {
        ParkingGuideAlgorithmState& next = control.nextState;
        next.errorFrameCount =
            (std::max)(0, previousState.errorFrameCount) + 1;
        const bool planeLost = IsPlaneLostError(localization.errorCode);
        next.lostFrameCount = planeLost
            ? (std::max)(0, previousState.lostFrameCount) + 1
            : 0;

        ParkingSpaceGuideInfo errorInfo = MakeLastKnownInfo(previousState);
        const int errorThreshold =
            (std::max)(1, processParam.errorFrameThreshold);
        if (next.errorFrameCount < errorThreshold) {
            const ParkingGuideState retainedState = previousState.hasMeasurement
                ? ParkingGuideStateFromCode(previousState.lastGuideStateCode)
                : ParkingGuideState::Waiting;
            SetGuideState(errorInfo,
                          retainedState == ParkingGuideState::Unknown
                              ? ParkingGuideState::Waiting
                              : retainedState);
            ApplyPublishedGuideMetrics(errorInfo);
            result.message = previousState.hasMeasurement
                ? QStringLiteral(
                      "距离 %1 mm，横向偏差 %2 mm，航向角 %3°，接近速度 %4 m/s")
                      .arg(errorInfo.distance, 0, 'f', 0)
                      .arg(errorInfo.lateralOffset, 0, 'f', 0)
                      .arg(errorInfo.angle, 0, 'f', 2)
                      .arg(errorInfo.aircraftSpeed, 0, 'f', 1)
                : QStringLiteral("等待飞机定位");
        } else {
            // Plane localization can briefly lose an aircraft while it is
            // entering the detection area. Keep reporting WAIT for that
            // expected condition instead of publishing state 14.
            SetGuideState(errorInfo,
                          planeLost
                              ? ParkingGuideState::Waiting
                              : ParkingGuideState::SystemError);
            ApplyPublishedGuideMetrics(errorInfo);
            errorInfo.hasException = !planeLost;
            result.errorCode = planeLost ? 0 : localization.errorCode;
            next.stoppedFrameCount = 0;
            next.stoppedShortFrameCount = 0;
            next.stopPositionReached = false;
            next.dockingCompleted = false;
            next.completedHoldFrameCount = 0;
            next.stopStableSinceMs = -1;
            result.message = localization.errorMessage.trimmed().isEmpty()
                ? QStringLiteral("停机引导定位失败")
                : localization.errorMessage;
        }
        result.parkingSpaceInfoList.push_back(errorInfo);
        return 0;
    }

    const ParkingGuideState previousGuideState =
        ParkingGuideStateFromCode(previousState.lastGuideStateCode);
    const double stopTolerance =
        (std::max)(0.0, processParam.stopDistanceTolerance);
    const double overshootDistance =
        (std::max)(stopTolerance, processParam.overshootDistance);
    const double slowDistance =
        (std::max)(stopTolerance, processParam.slowDistance);
    const double approachStartDistance =
        (std::max)(slowDistance, processParam.approachStartDistance);
    // 距离/横向突变过滤只用于对准中线和方位引导，捕获、到位和越位不再拦截。
    if (previousState.hasMeasurement &&
        ShouldFilterMeasurementJump(previousGuideState,
                                    localization.info.distance,
                                    stopTolerance,
                                    approachStartDistance)) {
        const double distanceChange = std::fabs(
            localization.info.distance - previousState.lastDistance);
        const double lateralOffsetChange = std::fabs(
            localization.info.deviation - previousState.lastDeviation);
        const double distanceThreshold = (std::max)(
            0.0, processParam.distanceChangeThreshold);
        const double lateralOffsetThreshold = (std::max)(
            0.0, processParam.lateralOffsetChangeThreshold);
        if (distanceChange > distanceThreshold ||
            lateralOffsetChange > lateralOffsetThreshold) {
            ParkingSpaceGuideInfo retainedInfo =
                MakeLastKnownInfo(previousState);
            const ParkingGuideState retainedState = previousGuideState;
            SetGuideState(retainedInfo,
                          retainedState == ParkingGuideState::Unknown
                              ? ParkingGuideState::Waiting
                              : retainedState);
            ApplyPublishedGuideMetrics(retainedInfo);
            result.parkingSpaceInfoList.push_back(retainedInfo);
            result.message = QStringLiteral(
                "检测数据突变已忽略：距离变化 %1 mm（阈值 %2 mm，%3），横向偏移变化 %4 mm（阈值 %5 mm，%6）")
                .arg(distanceChange, 0, 'f', 1)
                .arg(distanceThreshold, 0, 'f', 1)
                .arg(distanceChange > distanceThreshold
                         ? QStringLiteral("超阈值")
                         : QStringLiteral("未超阈值"))
                .arg(lateralOffsetChange, 0, 'f', 1)
                .arg(lateralOffsetThreshold, 0, 'f', 1)
                .arg(lateralOffsetChange > lateralOffsetThreshold
                         ? QStringLiteral("超阈值")
                         : QStringLiteral("未超阈值"));
            return 0;
        }
    }

    ParkingGuideAlgorithmState& next = control.nextState;
    next.errorFrameCount = 0;
    next.lostFrameCount = 0;
    next.successfulFrameCount =
        (std::max)(0, previousState.successfulFrameCount) + 1;

    double approachSpeed = previousState.filteredApproachSpeed;
    if (previousState.hasMeasurement &&
        previousState.errorFrameCount == 0 &&
        frameTimestampMs > previousState.lastTimestampMs) {
        const double elapsedSeconds =
            static_cast<double>(frameTimestampMs - previousState.lastTimestampMs) /
            1000.0;
        const double rawApproachSpeed =
            (previousState.lastDistance - localization.info.distance) /
            elapsedSeconds;
        const double alpha = std::clamp(processParam.speedFilterAlpha,
                                        0.0,
                                        1.0);
        approachSpeed = alpha * rawApproachSpeed +
                        (1.0 - alpha) * previousState.filteredApproachSpeed;
    } else if (!previousState.hasMeasurement ||
               previousState.errorFrameCount > 0) {
        approachSpeed = 0.0;
    }
    if (!std::isfinite(approachSpeed)) {
        approachSpeed = 0.0;
    }

    next.hasMeasurement = true;
    next.lastTimestampMs = frameTimestampMs;
    next.lastDistance = localization.info.distance;
    next.lastDeviation = localization.info.deviation;
    next.lastAngle = localization.info.dirAngle_deg;
    next.lastNoseX = localization.info.nosePoint.x;
    next.lastNoseY = localization.info.nosePoint.y;
    next.lastNoseZ = localization.info.nosePoint.z;
    next.filteredApproachSpeed = approachSpeed;

    ParkingSpaceGuidePosition position;
    position.x = localization.info.nosePoint.x;
    position.y = localization.info.nosePoint.y;
    position.z = localization.info.nosePoint.z;
    position.roll = 0.0;
    position.pitch = 0.0;
    position.yaw = localization.info.dirAngle_deg;
    result.positions.push_back(position);

    ParkingSpaceGuideInfo info = MakeLastKnownInfo(next);
    info.hasException = false;

    const double captureStartDistance =
        (std::max)(approachStartDistance,
                   processParam.captureStartDistance);
    const double dockingStartDistance =
        (std::max)(captureStartDistance,
                   processParam.dockingStartDistance);
    const double stoppedSpeed =
        (std::max)(0.0, processParam.stoppedSpeedThreshold);
    const double maxApproachSpeed =
        (std::max)(0.0, processParam.maxApproachSpeed);
    const bool hasSpeedBaseline = previousState.hasMeasurement &&
                                  previousState.errorFrameCount == 0;
    const bool isStopped = hasSpeedBaseline &&
                           std::fabs(approachSpeed) <= stoppedSpeed;
    const bool isAtStopPosition =
        std::fabs(localization.info.distance) <= stopTolerance;

    ParkingGuideState guideState = ParkingGuideState::Tracking;
    // Overshoot must win over a previously reached stop position.  The
    // completion latch is only valid while the aircraft remains in the stop
    // tolerance; otherwise the process must resume guidance.
    if (localization.info.distance <= -overshootDistance) {
        next.stopPositionReached = false;
        next.dockingCompleted = false;
        next.stoppedFrameCount = 0;
        next.stoppedShortFrameCount = 0;
        next.completedHoldFrameCount = 0;
        next.stopStableSinceMs = -1;
        next.centerLineAligned = false;
        guideState = ParkingGuideState::Overshot;
    } else if (previousState.dockingCompleted &&
               isAtStopPosition &&
               isStopped) {
        next.dockingCompleted = true;
        next.stopStableSinceMs = previousState.stopStableSinceMs;
        guideState = ParkingGuideState::DockingCompleted;
    } else if (isAtStopPosition) {
        next.dockingCompleted = false;
        next.stoppedShortFrameCount = 0;
        if (!previousState.stopPositionReached) {
            next.stopPositionReached = true;
            next.completedHoldFrameCount = 0;
            next.stopStableSinceMs = -1;
        }
        if (isStopped) {
            next.stoppedFrameCount =
                (std::max)(0, previousState.stoppedFrameCount) + 1;
        } else {
            next.stoppedFrameCount = 0;
            next.stopStableSinceMs = -1;
        }
        next.completedHoldFrameCount =
            (std::max)(0, previousState.completedHoldFrameCount) + 1;
        const int stableFrames = (std::max)(1, processParam.stopStableFrames);
        const int okHoldMs = (std::max)(0, processParam.okHoldMs);
        if (next.stoppedFrameCount >= stableFrames) {
            // 停稳后先保持 STOP，满 okHoldMs（默认 5 秒）再切到 OK。
            if (next.stopStableSinceMs < 0) {
                next.stopStableSinceMs = frameTimestampMs;
            }
            if (frameTimestampMs - next.stopStableSinceMs >= okHoldMs) {
                next.dockingCompleted = true;
                guideState = ParkingGuideState::DockingCompleted;
            } else {
                guideState = ParkingGuideState::StopPositionReached;
            }
        } else {
            next.stopStableSinceMs = -1;
            guideState = ParkingGuideState::StopPositionReached;
        }
    } else {
        next.stopPositionReached = false;
        next.dockingCompleted = false;
        next.stoppedFrameCount = 0;
        next.stoppedShortFrameCount = 0;
        next.centerLineAligned = false;
        next.completedHoldFrameCount = 0;
        next.stopStableSinceMs = -1;

        const double stoppedShortDistance =
            (std::max)(stopTolerance, processParam.stoppedShortMinDistance);
        if (previousState.hasMeasurement &&
            next.successfulFrameCount > 4 &&
            isStopped &&
            localization.info.distance >= stoppedShortDistance) {
            next.stoppedShortFrameCount =
                (std::max)(0, previousState.stoppedShortFrameCount) + 1;
        } else {
            next.stoppedShortFrameCount = 0;
        }

        if (next.stoppedShortFrameCount >=
            (std::max)(1, processParam.stoppedShortStableFrames)) {
            guideState = ParkingGuideState::StoppedShort;
        } else if (localization.info.distance >= 0.0 &&
                   localization.info.distance <= slowDistance &&
                   hasSpeedBaseline &&
                   maxApproachSpeed > 0.0 &&
                   approachSpeed >= maxApproachSpeed) {
            guideState = ParkingGuideState::Slow;
        } else if (localization.info.distance > slowDistance &&
                   hasSpeedBaseline &&
                   maxApproachSpeed > 0.0 &&
                   approachSpeed >= maxApproachSpeed) {
            guideState = ParkingGuideState::TooFast;
        } else if (localization.info.distance >= 0.0 &&
                   localization.info.distance <= approachStartDistance &&
                   (std::fabs(localization.info.deviation) >
                       (std::max)(0.0, guideParam.lateralTolerance) ||
                   std::fabs(localization.info.dirAngle_deg) >
                       (std::max)(0.0, guideParam.angleTolerance))) {
            next.centerLineAligned = false;
            guideState = ParkingGuideState::AzimuthGuidance;
        } else if (localization.info.distance >= 0.0 &&
                   localization.info.distance <= approachStartDistance) {
            next.centerLineAligned = true;
            guideState = ParkingGuideState::CenterLineAligned;
        } else if (localization.info.distance <= captureStartDistance &&
                   localization.info.distance > approachStartDistance) {
            next.centerLineAligned = false;
            guideState = ParkingGuideState::Capturing;
        } else if (localization.info.distance <= dockingStartDistance &&
                   localization.info.distance > captureStartDistance) {
            next.centerLineAligned = false;
            guideState = ParkingGuideState::DockingStarted;
        } else if (localization.info.distance > dockingStartDistance) {
            next.centerLineAligned = false;
            guideState = ParkingGuideState::Waiting;
        } else {
            next.centerLineAligned = false;
            guideState = ParkingGuideState::Tracking;
        }
    }

    SetGuideState(info, guideState);
    next.lastGuideStateCode = info.guideStateCode;
    if (ShouldClearPublishedSpeed(info.guideStateCode)) {
        next.filteredApproachSpeed = 0.0;
        info.aircraftSpeed = 0.0;
    }
    ApplyPublishedGuideMetrics(info);
    result.parkingSpaceInfoList.push_back(info);
    result.message = QStringLiteral(
        "距离 %1 mm，横向偏差 %2 mm，航向角 %3°，接近速度 %4 m/s")
        .arg(info.distance, 0, 'f', 0)
        .arg(info.lateralOffset, 0, 'f', 0)
        .arg(info.angle, 0, 'f', 2)
        .arg(info.aircraftSpeed, 0, 'f', 1);

    const double modelVerifyDistance = modelParam.modelVerifyDistance;
    const int maxRecognitionAttempts =
        (std::max)(1, modelParam.maxRecognitionAttempts);
    const bool isInModelVerifyRange = modelVerifyDistance >= 0.0 &&
        info.distance >= 0.0 &&
        info.distance <= modelVerifyDistance;
    const ParkingGuideState measuredState =
        ParkingGuideStateFromCode(info.guideStateCode);
    const bool mustPreserveMeasuredState =
        measuredState == ParkingGuideState::Overshot;
    const bool hasSelectedModel =
        !previousState.selectedModelType.trimmed().isEmpty();
    const bool requiresModelMatch =
        !DetectableModelFamily(previousState.selectedModelType).isEmpty();
    const bool canAttemptRecognition =
        !hasSelectedModel ||
        previousState.modelRecognitionAttempts < maxRecognitionAttempts;
    if (isInModelVerifyRange &&
        !previousState.modelRecognitionComplete &&
        !previousState.modelRecognitionInProgress &&
        canAttemptRecognition) {
        control.needModelRecognition = true;
        control.recognitionContext =
            previousState.selectedModelType.trimmed().toUtf8();
    } else if (isInModelVerifyRange &&
               !previousState.modelRecognitionComplete &&
               !previousState.modelRecognitionInProgress &&
               requiresModelMatch &&
               !mustPreserveMeasuredState) {
        ParkingSpaceGuideInfo& failedInfo = result.parkingSpaceInfoList.back();
        failedInfo.hasException = true;
        failedInfo.guideStateCode =
            static_cast<int>(ParkingGuideState::AircraftVerificationFailed);
        failedInfo.guideText =
            GuideText(ParkingGuideState::AircraftVerificationFailed);
        control.nextState.lastGuideStateCode = failedInfo.guideStateCode;
        result.message = QStringLiteral("机型二次验证失败");
    }

    return 0;
}

int DetectPresenter::RecognizeModel2D(const QImage& frame,
                                      const QByteArray& recognitionContext,
                                      ModelRecognitionResult& result)
{
    result = ModelRecognitionResult();
    if (!m_modelClassifier) {
        result.message = QStringLiteral("AAPGS机型识别器未初始化");
        return -1;
    }

    AapgsModelClassifier::Classification classification;
    QString errorMessage;
    if (!m_modelClassifier->Classify(frame, classification, errorMessage)) {
        result.message = errorMessage.trimmed().isEmpty()
            ? QStringLiteral("AAPGS机型识别失败")
            : errorMessage.trimmed();
        return -1;
    }

    result.recognitionComplete = true;
    result.verificationSupported = true;
    result.modelType = classification.modelType;
    result.confidence = classification.confidence;
    const QString expectedModelType =
        QString::fromUtf8(recognitionContext).trimmed();
    const QString recognizedModelType = result.modelType.trimmed();
    if (expectedModelType.isEmpty()) {
        result.message = QStringLiteral("自动识别机型：%1")
                             .arg(recognizedModelType);
        return 0;
    }
    // 检测只区分 A320 / B737；选择名包含对应家族即视为匹配，例如 A320-200 NEO。
    const QString expectedFamily = DetectableModelFamily(expectedModelType);
    if (expectedFamily.isEmpty()) {
        result.verificationSupported = false;
        result.message = QStringLiteral("选择机型%1不做 A320/B737 精确校验")
                             .arg(expectedModelType);
        return 0;
    }
    if (!SelectedModelMatchesDetection(expectedModelType, recognizedModelType)) {
        result.message = QStringLiteral("机型验证不一致：选择%1，识别%2")
                             .arg(expectedModelType,
                                  recognizedModelType.isEmpty()
                                      ? QStringLiteral("未知")
                                      : recognizedModelType);
        return 0;
    }

    result.verified = true;
    return 0;
}

int DetectPresenter::DetectPersonInRegion2D(
    const QImage& frame,
    const VrPersonDetectionParam& personParam,
    PersonDetectionResult& result)
{
    result = PersonDetectionResult();
    if (!m_personDetector || frame.isNull()) {
        result.message = QStringLiteral("人员检测图像或检测器不可用");
        return -1;
    }

    const double imageWidth = static_cast<double>(frame.width());
    const double imageHeight = static_cast<double>(frame.height());
    // The caller has already cropped the configured chock ROI. Detection and
    // result coordinates therefore use the cropped image as their full area.
    result.roi = QRectF(0.0, 0.0, imageWidth, imageHeight);

    PersionModelDetector::Analysis analysis;
    QString errorMessage;
    if (!m_personDetector->Analyze(frame, analysis, errorMessage)) {
        result.message = errorMessage.trimmed().isEmpty()
            ? QStringLiteral("Persion人员检测失败")
            : errorMessage.trimmed();
        return -1;
    }

    const double threshold = std::clamp(personParam.confidenceThreshold,
                                        0.0,
                                        1.0);
    for (const PersionModelDetector::Detection& detection : analysis.detections) {
        if (!IsPersonDetection(detection) ||
            detection.confidence < threshold) {
            continue;
        }

        const QRectF box = detection.boundingBox.intersected(
            QRectF(0.0, 0.0, imageWidth, imageHeight));
        if (box.isEmpty()) {
            continue;
        }
        result.personBoxes.push_back(box);

        ++result.personCount;
        result.confidence = (std::max)(result.confidence,
                                       detection.confidence);
    }

    result.personInRegion = result.personCount > 0;
    result.message = result.personInRegion
        ? QStringLiteral("轮挡区域检测到人员")
        : QStringLiteral("等待人员进入轮挡区域");
    return 0;
}