using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;

/// <summary>
/// 单次 Hybrid A* 搜索的只读结果。
/// 即使失败也会保留已经创建的运行期节点，供上层记录诊断；仅 <see cref="PlanningStatus.Success"/> 时 <see cref="SuccessNodeIndex"/> 有值。
/// </summary>
public sealed class HybridAStarSearchResult
{
    internal HybridAStarSearchResult(
        PlanningStatus status,
        IEnumerable<HybridAStarNode> nodes,
        int expandedNodeCount,
        int generatedNodeCount,
        int reopenedNodeCount,
        int staleOpenListEntryCount,
        int peakOpenListCount,
        int? successNodeIndex,
        string terminationReason)
    {
        Status = status;
        Nodes = new ReadOnlyCollection<HybridAStarNode>(new List<HybridAStarNode>(nodes ?? Array.Empty<HybridAStarNode>()));
        ExpandedNodeCount = expandedNodeCount;
        GeneratedNodeCount = generatedNodeCount;
        ReopenedNodeCount = reopenedNodeCount;
        StaleOpenListEntryCount = staleOpenListEntryCount;
        PeakOpenListCount = peakOpenListCount;
        SuccessNodeIndex = status == PlanningStatus.Success ? successNodeIndex : null;
        TerminationReason = status == PlanningStatus.Success ? string.Empty : terminationReason ?? string.Empty;
    }

    /// <summary>搜索终止状态。</summary>
    public PlanningStatus Status { get; }

    /// <summary>本次搜索已经创建的全部节点；索引与 <see cref="HybridAStarNode.NodeIndex"/> 一致。</summary>
    public IReadOnlyList<HybridAStarNode> Nodes { get; }

    /// <summary>实际从 Open List 弹出并扩展的节点数量。</summary>
    public int ExpandedNodeCount { get; }

    /// <summary>已进入 Open List 的节点数量，包含根节点和因改进代价追加的节点。</summary>
    public int GeneratedNodeCount { get; }

    /// <summary>更优路径到达已关闭离散状态、并重新放回 Open List 的次数。</summary>
    public int ReopenedNodeCount { get; }

    /// <summary>从 Open List 弹出后因已有更优普通状态而被丢弃的陈旧条目数量。</summary>
    public int StaleOpenListEntryCount { get; }

    /// <summary>搜索期间 Open List 持有的最大条目数，包含等待惰性丢弃的旧条目。</summary>
    public int PeakOpenListCount { get; }

    /// <summary>成功时最后一个从 Open List 弹出且满足终点条件的节点索引；失败时为 null。</summary>
    public int? SuccessNodeIndex { get; }

    /// <summary>搜索边界记录的原始终止原因；成功时为空字符串，失败时非空。</summary>
    public string TerminationReason { get; }
}

/// <summary>
/// 在不可变 <see cref="PlanningGridMap"/> 上执行前进/倒车恒曲率原语的 Hybrid A* 搜索。
/// 搜索只消费已准备好的地图快照；所有候选原语均先完成连续扩大车体碰撞检查，再参与 Open List 排序。
/// </summary>
public sealed class HybridAStarSearch
{
    private const double CostImprovementToleranceMeters = 1e-9d;
    private readonly MotionPrimitiveGenerator _primitiveGenerator;
    private readonly SearchCostCalculator _costCalculator;
    private readonly FootprintCollisionChecker _collisionChecker;

    /// <summary>创建使用默认原语、代价和碰撞检查实现的搜索器。</summary>
    public HybridAStarSearch()
        : this(new MotionPrimitiveGenerator(), new SearchCostCalculator(), new FootprintCollisionChecker())
    {
    }

    /// <summary>创建使用指定协作对象的搜索器，便于在不引入地图或 UI 依赖的情况下测试搜索过程。</summary>
    public HybridAStarSearch(
        MotionPrimitiveGenerator primitiveGenerator,
        SearchCostCalculator costCalculator,
        FootprintCollisionChecker collisionChecker)
    {
        _primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
        _costCalculator = costCalculator ?? throw new ArgumentNullException(nameof(costCalculator));
        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
    }

    /// <summary>
    /// 执行一次不可变地图上的 Hybrid A* 搜索。
    /// 参数：request 提供地图、起终点、车辆和搜索配置；cancellationToken 在每次节点扩展前检查。
    /// 返回：成功仅在满足目标约束的候选已从 Open List 弹出时报告；任一失败状态均不报告成功节点。
    /// </summary>
    public HybridAStarSearchResult Search(PlanningRequest request, CancellationToken cancellationToken)
    {
        TimeSpan timeout = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
            ? request.Configuration.SearchTimeout
            : TimeSpan.Zero;
        PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
            ? new PlanningOperationBudget(cancellationToken, timeout)
            : PlanningOperationBudget.Unlimited(cancellationToken);
        return Search(request, budget);
    }

    /// <summary>使用门面传入的共享预算执行搜索；预算从整次规划开始计时。</summary>
    internal HybridAStarSearchResult Search(PlanningRequest request, PlanningOperationBudget budget)
    {
        var nodes = new List<HybridAStarNode>();
        int expandedNodeCount = 0;
        int generatedNodeCount = 0;
        int reopenedNodeCount = 0;
        int staleOpenListEntryCount = 0;
        int peakOpenListCount = 0;

        try
        {
            if (budget == null) throw new ArgumentNullException(nameof(budget));
            PlanningOperationStopReason stopReason = budget.GetStopReason();
            if (stopReason != PlanningOperationStopReason.None)
                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);

            PlanningStatus validationStatus = ValidateRequest(request);
            if (validationStatus != PlanningStatus.Success)
                return CreateResult(validationStatus, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);

            PlanningGridMap map = request.Map;
            HybridAStarConfiguration configuration = request.Configuration;
            VehicleParameters vehicle = request.Vehicle;

            if (!IsFootprintInsideMap(request.Start, map, vehicle))
                return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);
            if (!_collisionChecker.IsPoseCollisionFree(request.Start, map, vehicle, 0d, out _))
                return CreateResult(PlanningStatus.StartInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);
            if (!IsFootprintInsideMap(request.Goal, map, vehicle))
                return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);
            if (!_collisionChecker.IsPoseCollisionFree(request.Goal, map, vehicle, 0d, out _))
                return CreateResult(PlanningStatus.GoalInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);

            stopReason = budget.GetStopReason();
            if (stopReason != PlanningOperationStopReason.None)
                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);
            if (configuration.MaximumExpandedNodes == 0)
                return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);

            if (!map.TryWorldToGrid(request.Goal.X, request.Goal.Y, out int goalRow, out int goalColumn))
                return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);
            if (!GridDijkstraHeuristic.TryCreate(map, goalRow, goalColumn, budget, out GridDijkstraHeuristic heuristic, out stopReason))
                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);
            if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
                return CreateResult(PlanningStatus.InvalidVehicleParameters, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);

            IReadOnlyList<double> curvatureLevels = _primitiveGenerator.GetCurvatureLevels(vehicle, configuration);
            if (curvatureLevels.Count == 0)
                return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);

            int headingBinCount = GetHeadingBinCount(configuration.HeadingResolutionRadians);
            int startCurvatureLevelIndex = GetNearestCurvatureLevelIndex(curvatureLevels, request.StartVehicleCurvature);
            if (startCurvatureLevelIndex < 0)
                return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);

            if (!map.TryWorldToGrid(request.Start.X, request.Start.Y, out int startRow, out int startColumn))
                return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null);

            var openList = new BinaryMinHeap<int>();
            var bestStates = new Dictionary<HybridAStarNodeKey, NodeLabel>();
            foreach (TravelDirection startDirection in GetStartDirections(request, configuration))
            {
                int headingIndex = AngleMath.ToHeadingIndex(request.Start.Heading, configuration.HeadingResolutionRadians, headingBinCount);
                var key = new HybridAStarNodeKey(startRow, startColumn, headingIndex, startDirection, startCurvatureLevelIndex);
                if (bestStates.ContainsKey(key)) continue;

                double hCostMeters = GetHeuristicCost(heuristic, startRow, startColumn, configuration);
                if (double.IsPositiveInfinity(hCostMeters)) continue;
                var node = new HybridAStarNode(nodes.Count, key, request.Start, -1, null,
                    curvatureLevels[startCurvatureLevelIndex], 0d, hCostMeters);
                nodes.Add(node);
                bestStates.Add(key, new NodeLabel(node.NodeIndex, 0d, false));
                openList.Push(node.NodeIndex, node.FCostMeters, node.HCostMeters, node.GCostMeters);
                peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
                generatedNodeCount++;
            }

            if (openList.Count == 0)
                return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                    staleOpenListEntryCount, peakOpenListCount, null,
                    "二维启发式标记起点不可达目标，或起始方向无法进入 Open List。");

            while (openList.Count > 0)
            {
                stopReason = budget.GetStopReason();
                if (stopReason != PlanningOperationStopReason.None)
                    return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                        staleOpenListEntryCount, peakOpenListCount, null);
                if (expandedNodeCount >= configuration.MaximumExpandedNodes)
                    return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                        staleOpenListEntryCount, peakOpenListCount, null);

                int nodeIndex = openList.Pop();
                HybridAStarNode current = nodes[nodeIndex];
                NodeLabel currentLabel = null;
                if (!current.IsGoalCandidate)
                {
                    if (!bestStates.TryGetValue(current.Key, out currentLabel) || currentLabel.NodeIndex != nodeIndex || currentLabel.IsClosed)
                    {
                        staleOpenListEntryCount++;
                        continue;
                    }

                    currentLabel.IsClosed = true;
                }

                expandedNodeCount++;
                if (current.IsGoalCandidate)
                {
                    if (!GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection) ||
                        !_collisionChecker.IsPoseCollisionFree(current.Pose, map, vehicle, 0d, out _))
                        continue;

                    return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                        staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
                }

                if (GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection))
                    return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                        staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);

                for (int curvatureLevelIndex = 0; curvatureLevelIndex < curvatureLevels.Count; curvatureLevelIndex++)
                {
                    stopReason = budget.GetStopReason();
                    if (stopReason != PlanningOperationStopReason.None)
                        return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                            staleOpenListEntryCount, peakOpenListCount, null);
                    if (!MotionPrimitiveGenerator.AreCurvatureLevelsAdjacent(current.CurvatureLevelIndex, curvatureLevelIndex)) continue;

                    foreach (TravelDirection direction in GetSuccessorDirections(configuration))
                    {
                        MotionPrimitive primitive = _primitiveGenerator.Generate(current.Pose, curvatureLevels[curvatureLevelIndex], direction, request);
                        if (primitive == null || primitive.ActualLengthMeters <= 0d) continue;

                        if (!map.TryWorldToGrid(primitive.End.X, primitive.End.Y, out int row, out int column)) continue;
                        double hCostMeters = GetHeuristicCost(heuristic, row, column, configuration);
                        if (double.IsPositiveInfinity(hCostMeters)) continue;

                        double bodyClearanceMeters = GetMinimumBodyClearance(primitive);
                        bool isGearSwitch = current.ParentNodeIndex >= 0 && current.Direction != direction;
                        int curvatureLevelDelta = curvatureLevelIndex - current.CurvatureLevelIndex;
                        double incrementalCostMeters = _costCalculator.Calculate(
                            primitive.ActualLengthMeters,
                            direction,
                            isGearSwitch,
                            curvatureLevels[curvatureLevelIndex],
                            maximumCurvaturePerMeter,
                            curvatureLevelDelta,
                            bodyClearanceMeters,
                            configuration);
                        double gCostMeters = current.GCostMeters + incrementalCostMeters;
                        if (!NumericGuard.IsFinite(gCostMeters) || gCostMeters < 0d) continue;

                        int headingIndex = AngleMath.ToHeadingIndex(primitive.End.Heading, configuration.HeadingResolutionRadians, headingBinCount);
                        var key = new HybridAStarNodeKey(row, column, headingIndex, direction, curvatureLevelIndex);
                        bestStates.TryGetValue(key, out NodeLabel existingLabel);
                        var successor = new HybridAStarNode(nodes.Count, key, primitive.End, current.NodeIndex, primitive,
                            curvatureLevels[curvatureLevelIndex], gCostMeters, hCostMeters, primitive.IsGoalTruncation);
                        if (existingLabel != null && !ShouldEnqueueSuccessor(successor, existingLabel.GCostMeters)) continue;

                        if (!successor.IsGoalCandidate && existingLabel != null && existingLabel.IsClosed) reopenedNodeCount++;
                        nodes.Add(successor);
                        if (!successor.IsGoalCandidate)
                            bestStates[key] = new NodeLabel(successor.NodeIndex, gCostMeters, false);
                        openList.Push(successor.NodeIndex, successor.FCostMeters, successor.HCostMeters, successor.GCostMeters);
                        peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
                        generatedNodeCount++;
                    }
                }
            }

            return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                staleOpenListEntryCount, peakOpenListCount, null,
                "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。");
        }
        catch (Exception exception)
        {
            string reason = "Hybrid A* 搜索内部错误：" + exception.GetType().Name +
                (string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
            return CreateResult(PlanningStatus.InternalError, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
                staleOpenListEntryCount, peakOpenListCount, null, reason);
        }
    }

    private static HybridAStarSearchResult CreateResult(
        PlanningStatus status,
        IEnumerable<HybridAStarNode> nodes,
        int expandedNodeCount,
        int generatedNodeCount,
        int reopenedNodeCount,
        int staleOpenListEntryCount,
        int peakOpenListCount,
        int? successNodeIndex,
        string terminationReason = null)
    {
        string reason = status == PlanningStatus.Success
            ? string.Empty
            : terminationReason ?? GetDefaultTerminationReason(status);
        return new HybridAStarSearchResult(status, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
            staleOpenListEntryCount, peakOpenListCount, successNodeIndex, reason);
    }

    private static string GetDefaultTerminationReason(PlanningStatus status)
    {
        switch (status)
        {
            case PlanningStatus.Cancelled:
                return "Hybrid A* 搜索已取消。";
            case PlanningStatus.InvalidRequest:
                return "Hybrid A* 搜索请求缺少必要对象或包含非法数值。";
            case PlanningStatus.InvalidMap:
                return "Hybrid A* 搜索地图结构无效。";
            case PlanningStatus.MapNotReady:
                return "Hybrid A* 搜索地图尚未准备好。";
            case PlanningStatus.InvalidVehicleParameters:
                return "Hybrid A* 搜索车辆参数无效。";
            case PlanningStatus.InvalidCurvatureConfiguration:
                return "Hybrid A* 搜索曲率、离散、代价或资源配置无效。";
            case PlanningStatus.StartOutsideMap:
                return "Hybrid A* 搜索起始扩大车体不完全位于地图内。";
            case PlanningStatus.StartInCollision:
                return "Hybrid A* 搜索起始扩大车体与障碍物相交或擦边。";
            case PlanningStatus.GoalOutsideMap:
                return "Hybrid A* 搜索目标扩大车体不完全位于地图内。";
            case PlanningStatus.GoalInCollision:
                return "Hybrid A* 搜索目标扩大车体与障碍物相交或擦边。";
            case PlanningStatus.SearchTimeout:
                return "Hybrid A* 搜索使用的总规划预算已耗尽。";
            case PlanningStatus.SearchNodeLimitExceeded:
                return "Hybrid A* 搜索达到扩展节点上限。";
            case PlanningStatus.NoFeasiblePath:
                return "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。";
            case PlanningStatus.BacktrackingFailed:
                return "Hybrid A* 成功节点无法回溯为完整父链。";
            case PlanningStatus.FinalValidationFailed:
                return "Hybrid A* 路径未通过最终复核。";
            case PlanningStatus.InternalError:
                return "Hybrid A* 搜索发生未预期内部错误。";
            default:
                return "Hybrid A* 搜索以未识别状态终止：" + status + "。";
        }
    }

    private static PlanningStatus ValidateRequest(PlanningRequest request)
    {
        if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
            !IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
            !IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
            return PlanningStatus.InvalidRequest;

        PlanningGridMap map = request.Map;
        if (map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters) || map.Bounds == null)
            return PlanningStatus.InvalidMap;
        if (!map.PlanningReady) return PlanningStatus.MapNotReady;

        VehicleParameters vehicle = request.Vehicle;
        if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
            return PlanningStatus.InvalidVehicleParameters;

        HybridAStarConfiguration configuration = request.Configuration;
        if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
            (request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
            return PlanningStatus.InvalidCurvatureConfiguration;

        return PlanningStatus.Success;
    }

    private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
    {
        return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
            NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
            NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
            NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
            configuration.HeadingResolutionRadians <= 2d * Math.PI &&
            configuration.CurvatureLevelCount >= 3 && configuration.CurvatureLevelCount % 2 == 1 &&
            NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) && configuration.GoalPositionToleranceMeters >= 0d &&
            NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) && configuration.GoalHeadingToleranceRadians >= 0d &&
            configuration.MaximumExpandedNodes >= 0 && configuration.SearchTimeout >= TimeSpan.Zero &&
            NumericGuard.IsFinite(configuration.HeuristicWeight) && configuration.HeuristicWeight >= 0d &&
            NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
            NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
            NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
            NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) && configuration.CurvatureChangePenaltyMetersPerLevel >= 0d &&
            NumericGuard.IsFinite(configuration.ClearanceCostWeight) && configuration.ClearanceCostWeight >= 0d &&
            NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
    }

    private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
    {
        if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;

        double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
        double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
        double longitudinalX = Math.Cos(pose.Heading);
        double longitudinalY = Math.Sin(pose.Heading);
        double lateralX = -longitudinalY;
        double lateralY = longitudinalX;
        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
        {
            double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
            double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
        }
        return true;
    }

    private static IEnumerable<TravelDirection> GetStartDirections(PlanningRequest request, HybridAStarConfiguration configuration)
    {
        if (request.StartDirection.HasValue)
        {
            yield return request.StartDirection.Value;
            yield break;
        }

        yield return TravelDirection.Forward;
        if (configuration.AllowReverse) yield return TravelDirection.Reverse;
    }

    private static IEnumerable<TravelDirection> GetSuccessorDirections(HybridAStarConfiguration configuration)
    {
        yield return TravelDirection.Forward;
        if (configuration.AllowReverse) yield return TravelDirection.Reverse;
    }

    private static int GetHeadingBinCount(double headingResolutionRadians)
    {
        double rawBinCount = Math.Ceiling(2d * Math.PI / headingResolutionRadians);
        if (!NumericGuard.IsFinite(rawBinCount) || rawBinCount < 1d || rawBinCount > int.MaxValue)
            throw new ArgumentOutOfRangeException(nameof(headingResolutionRadians));
        return (int)rawBinCount;
    }

    private static int GetNearestCurvatureLevelIndex(IReadOnlyList<double> curvatureLevels, double curvaturePerMeter)
    {
        int bestIndex = -1;
        double smallestDifference = double.PositiveInfinity;
        for (int index = 0; index < curvatureLevels.Count; index++)
        {
            double difference = Math.Abs(curvatureLevels[index] - curvaturePerMeter);
            if (difference >= smallestDifference) continue;
            smallestDifference = difference;
            bestIndex = index;
        }
        return bestIndex;
    }

    private static double GetMinimumBodyClearance(MotionPrimitive primitive)
    {
        double minimum = double.PositiveInfinity;
        foreach (double clearanceMeters in primitive.BodyClearancesMeters)
        {
            if (double.IsNaN(clearanceMeters) || clearanceMeters < 0d) return 0d;
            minimum = Math.Min(minimum, clearanceMeters);
        }
        return minimum;
    }

    private static double GetHeuristicCost(GridDijkstraHeuristic heuristic, int row, int column, HybridAStarConfiguration configuration)
    {
        double gridCostMeters = heuristic.GetCost(row, column);
        if (double.IsPositiveInfinity(gridCostMeters)) return gridCostMeters;
        double weightedCostMeters = gridCostMeters * configuration.HeuristicWeight;
        return NumericGuard.IsFinite(weightedCostMeters) && weightedCostMeters >= 0d
            ? weightedCostMeters
            : double.PositiveInfinity;
    }

    /// <summary>
    /// 判断后继是否应进入 Open List。
    /// 参数：successor 是已完成连续碰撞检查的后继；bestKnownGCostMeters 是相同离散键普通状态的当前最优 G 值。
    /// 返回：普通状态仅在严格改善最优 G 值时入队；终点候选始终入队，以保留其未量化的连续终点位姿。
    /// </summary>
    private static bool ShouldEnqueueSuccessor(HybridAStarNode successor, double bestKnownGCostMeters)
    {
        if (successor == null || !NumericGuard.IsFinite(bestKnownGCostMeters)) return false;
        return successor.IsGoalCandidate || successor.GCostMeters < bestKnownGCostMeters - CostImprovementToleranceMeters;
    }

    private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
    {
        if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
        if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
        throw new ArgumentOutOfRangeException(nameof(stopReason));
    }

    private static bool IsFinitePose(Pose2D pose)
    {
        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
    }

    private static bool IsTravelDirection(TravelDirection direction)
    {
        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
    }

    private static bool IsGoalDirection(GoalDirectionConstraint direction)
    {
        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
    }

    private sealed class NodeLabel
    {
        public NodeLabel(int nodeIndex, double gCostMeters, bool isClosed)
        {
            NodeIndex = nodeIndex;
            GCostMeters = gCostMeters;
            IsClosed = isClosed;
        }

        public int NodeIndex { get; }
        public double GCostMeters { get; }
        public bool IsClosed { get; set; }
    }
}