GrabBag/App/ParkingSpaceGuide/ParkingSpaceGuideApp/Presenter/Src/DetectPresenter.cpp

#include "DetectPresenter.h"

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

#include "AapgsModelClassifier.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 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;
}

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("OVERSHOOT");
    case ParkingGuideState::StoppedShort: return QStringLiteral("STOP SHORT");
    case ParkingGuideState::SlowAircraftLost: return QStringLiteral("SLOW+LOST");
    case ParkingGuideState::TooFast: return QStringLiteral("TOO FAST");
    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;
    info.modelType = state.modelVerified ? state.modelType
                                         : QStringLiteral("未知");
    info.distance = state.lastDistance;
    info.lateralOffset = state.lastDeviation;
    info.angle = state.lastAngle;
    info.aircraftSpeed = state.filteredApproachSpeed / 1000.0;
    info.confidence = state.modelVerified ? state.modelConfidence : 0.0;
    return info;
}
}

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

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 1.0.0")
        .arg(planeVersion ? QString::fromLocal8Bit(planeVersion)
                          : QStringLiteral("未知"));
}

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;
    const PlaneLocalizationResult localization =
        LocalizePlane(cloud, algorithmParams);

    if (localization.errorCode != 0) {
        ParkingGuideAlgorithmState& next = control.nextState;
        next.lostFrameCount = (std::max)(0, previousState.lostFrameCount) + 1;
        next.stoppedFrameCount = 0;
        next.stoppedShortFrameCount = 0;
        ParkingSpaceGuideInfo lostInfo = MakeLastKnownInfo(previousState);
        const int lostThreshold = (std::max)(1, processParam.lostFrameThreshold);
        const bool planeLost = IsPlaneLostError(localization.errorCode);
        if (planeLost) {
            if (!previousState.hasMeasurement) {
                SetGuideState(lostInfo, ParkingGuideState::Waiting);
            } else if (next.lostFrameCount < lostThreshold) {
                ParkingGuideState transientState =
                    previousState.lastDistance <= processParam.slowDistance
                    ? ParkingGuideState::Slow
                    : ParkingGuideStateFromCode(previousState.lastGuideStateCode);
                if (transientState == ParkingGuideState::Unknown ||
                    transientState == ParkingGuideState::StopPositionReached ||
                    transientState == ParkingGuideState::DockingCompleted ||
                    transientState == ParkingGuideState::Overshot ||
                    transientState == ParkingGuideState::StoppedShort) {
                    transientState = ParkingGuideState::Tracking;
                }
                SetGuideState(lostInfo, transientState);
            } else {
                SetGuideState(lostInfo, ParkingGuideState::SlowAircraftLost);
                lostInfo.hasException = true;
                result.errorCode = localization.errorCode;
            }
        } else {
            SetGuideState(lostInfo, ParkingGuideState::SystemError);
            lostInfo.hasException = true;
            result.errorCode = localization.errorCode;
        }
        result.parkingSpaceInfoList.push_back(lostInfo);
        result.message = localization.errorMessage.trimmed().isEmpty()
            ? QStringLiteral("停机引导定位失败")
            : localization.errorMessage;
        return 0;
    }

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

    double approachSpeed = previousState.filteredApproachSpeed;
    if (previousState.hasMeasurement &&
        previousState.lostFrameCount == 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.lostFrameCount > 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 stopTolerance =
        (std::max)(0.0, processParam.stopDistanceTolerance);
    const double stoppedSpeed =
        (std::max)(0.0, processParam.stoppedSpeedThreshold);
    const double maxApproachSpeed =
        (std::max)(0.0, processParam.maxApproachSpeed);
    const bool hasSpeedBaseline = previousState.hasMeasurement &&
                                  previousState.lostFrameCount == 0;
    const bool isStopped = hasSpeedBaseline &&
                           std::fabs(approachSpeed) <= stoppedSpeed;
    const bool isAtStopPosition =
        std::fabs(localization.info.distance) <= stopTolerance;

    ParkingGuideState guideState = ParkingGuideState::Tracking;
    if (previousState.dockingCompleted &&
        (!isAtStopPosition || !isStopped)) {
        next.dockingCompleted = false;
        next.stopPositionReached = false;
        next.completedHoldFrameCount = 0;
    }

    if (localization.info.distance < -stopTolerance) {
        next.stopPositionReached = false;
        next.dockingCompleted = false;
        next.stoppedFrameCount = 0;
        next.stoppedShortFrameCount = 0;
        next.centerLineAligned = false;
        guideState = ParkingGuideState::Overshot;
    } else if (approachSpeed > maxApproachSpeed &&
               maxApproachSpeed > 0.0) {
        next.dockingCompleted = false;
        guideState = ParkingGuideState::TooFast;
    } else if (next.dockingCompleted) {
        guideState = ParkingGuideState::DockingCompleted;
    } else if (isAtStopPosition) {
        next.stoppedShortFrameCount = 0;
        if (!previousState.stopPositionReached) {
            next.stopPositionReached = true;
            next.completedHoldFrameCount = 0;
        }
        if (isStopped) {
            next.stoppedFrameCount =
                (std::max)(0, previousState.stoppedFrameCount) + 1;
        } else {
            next.stoppedFrameCount = 0;
        }
        next.completedHoldFrameCount =
            (std::max)(0, previousState.completedHoldFrameCount) + 1;
        const int stableFrames = (std::max)(1, processParam.stopStableFrames);
        const int completedFrames =
            (std::max)(1, processParam.completedHoldFrames);
        if (next.stoppedFrameCount >= stableFrames &&
            next.completedHoldFrameCount >= completedFrames) {
            next.dockingCompleted = true;
            guideState = ParkingGuideState::DockingCompleted;
        } else {
            guideState = ParkingGuideState::StopPositionReached;
        }
    } else {
        next.stopPositionReached = false;
        next.stoppedFrameCount = 0;
        next.completedHoldFrameCount = 0;

        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 (next.successfulFrameCount == 1) {
            guideState = ParkingGuideState::DockingStarted;
        } else if (next.successfulFrameCount == 2) {
            guideState = ParkingGuideState::Capturing;
        } else if (next.successfulFrameCount == 3) {
            guideState = ParkingGuideState::Tracking;
        } else if (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 (!previousState.centerLineAligned) {
            next.centerLineAligned = true;
            guideState = ParkingGuideState::CenterLineAligned;
        } else if (localization.info.distance <=
                   (std::max)(stopTolerance, processParam.slowDistance)) {
            guideState = ParkingGuideState::Slow;
        } else if (localization.info.distance <=
                       (std::max)(processParam.slowDistance,
                                  processParam.approachStartDistance) &&
                   previousState.hasMeasurement &&
                   approachSpeed > 0.0) {
            guideState = ParkingGuideState::ApproachRate;
        } else {
            guideState = ParkingGuideState::Tracking;
        }
    }

    SetGuideState(info, guideState);
    next.lastGuideStateCode = info.guideStateCode;
    result.parkingSpaceInfoList.push_back(info);
    result.message = QStringLiteral(
        "距离 %1 mm，横向偏差 %2 mm，航向角 %3°，接近速度 %4 m/s")
        .arg(info.distance, 0, 'f', 1)
        .arg(info.lateralOffset, 0, 'f', 1)
        .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;
    if (isInModelVerifyRange &&
        !previousState.modelVerified &&
        previousState.modelRecognitionAttempts < maxRecognitionAttempts) {
        control.needModelRecognition = true;
        control.recognitionContext = QByteArrayLiteral("target-index=0");
        control.nextState.modelRecognitionAttempts =
            (std::max)(0, previousState.modelRecognitionAttempts) + 1;
    } else if (isInModelVerifyRange && !previousState.modelVerified) {
        ParkingSpaceGuideInfo& failedInfo = result.parkingSpaceInfoList.back();
        failedInfo.hasException = true;
        failedInfo.guideStateCode =
            static_cast<int>(ParkingGuideState::AircraftVerificationFailed);
        failedInfo.guideText = QStringLiteral("STOP+IDFAIL");
        result.message = QStringLiteral("机型二次验证失败");
    }

    return 0;
}

int DetectPresenter::RecognizeModel2D(const QImage& frame,
                                      const QByteArray& recognitionContext,
                                      ModelRecognitionResult& result)
{
    Q_UNUSED(recognitionContext);

    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.modelType = classification.modelType;
    result.confidence = classification.confidence;
    result.verified = true;
    return 0;
}