zengzy / hhrcs_service · Files

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text.Json;
using System.Threading;
using System.Threading.Tasks;
using Microsoft.Extensions.DependencyInjection;
using Microsoft.Extensions.Logging;
using Microsoft.Extensions.Options;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
using Rcs.Application.Services.PathFind.Realtime;
using Rcs.Application.Services.Protocol;
using Rcs.Application.Shared;
using Rcs.Domain.Entities;
using Rcs.Domain.Enums;
using Rcs.Domain.Models.VDA5050;
using Rcs.Domain.Repositories;
using Rcs.Domain.Settings;
using Rcs.Domain.ValueObjects;
using Rcs.Infrastructure.PathFinding.Services;
using StackExchange.Redis;

namespace Rcs.Infrastructure.PathFinding.Realtime;

/// <summary>
/// 空闲机器人让行服务实现。
/// 当执行任务的机器人路径被空闲机器人阻挡时，主动请求空闲机器人移开。
/// @author zzy
/// </summary>
public class IdleRobotYieldService : IIdleRobotYieldService
{
    private const string YieldCooldownKeyPrefix = "rcs:yield:cooldown";
    private const string YieldKeyMarker = "yield";

    private readonly IRobotCacheService _robotCacheService;
    private readonly IAgvPathService _agvPathService;
    private readonly IUnifiedTrafficControlService _trafficControl;
    private readonly IVdaPathSegmentationService _vdaPathSegmentationService;
    private readonly IServiceProvider _serviceProvider;
    private readonly IRobotRepository _robotRepository;
    private readonly IConnectionMultiplexer _redis;
    private readonly IVdaOrderGateService _vdaOrderGateService;
    private readonly ILogger<IdleRobotYieldService> _logger;
    private readonly AppSettings _settings;

    public IdleRobotYieldService(
        IRobotCacheService robotCacheService,
        IAgvPathService agvPathService,
        IUnifiedTrafficControlService trafficControl,
        IVdaPathSegmentationService vdaPathSegmentationService,
        IServiceProvider serviceProvider,
        IRobotRepository robotRepository,
        IConnectionMultiplexer redis,
        IVdaOrderGateService vdaOrderGateService,
        ILogger<IdleRobotYieldService> logger,
        IOptions<AppSettings> settings)
    {
        _robotCacheService = robotCacheService;
        _agvPathService = agvPathService;
        _trafficControl = trafficControl;
        _vdaPathSegmentationService = vdaPathSegmentationService;
        _serviceProvider = serviceProvider;
        _robotRepository = robotRepository;
        _redis = redis;
        _vdaOrderGateService = vdaOrderGateService;
        _logger = logger;
        _settings = settings.Value;
    }

    /// <inheritdoc />
    public async Task<IdleYieldResult> ExecuteYieldAsync(
        IdleRobotYieldRequest request,
        Robot requestingRobot,
        VdaSegmentedPathCache requestingCache,
        CancellationToken ct = default)
    {
        try
        {
            // 1. 获取空闲机器人信息
            var robot = await _robotRepository.GetByIdAsync(request.IdleRobotId, ct) as Robot;
            if (robot == null || !robot.Active)
            {
                return new IdleYieldResult { Success = false, FailureReason = "Robot not found or inactive" };
            }

            var taskSnapshot = await GetIdleRobotTaskSnapshotAsync(robot.RobotId, ct);
            if (taskSnapshot.HasInProgressTaskOrSubTask)
            {
                _logger.LogInformation(
                    "[让行] 让行车存在执行中任务或子任务，拒绝按空闲车处理: IdleRobot={IdleRobotId}",
                    robot.RobotId);
                return new IdleYieldResult
                {
                    Success = false,
                    FailureReason = "Robot has in-progress task/subtask, not idle"
                };
            }

            // 2. 若已存在进行中的让行任务，优先续发当前让行，避免重复创建新让行订单。
            var (activeYieldCacheKey, activeYieldCache) = await GetLatestYieldCacheAsync(robot.RobotId, ct);
            if (activeYieldCache != null && !string.IsNullOrWhiteSpace(activeYieldCacheKey))
            {
                if (IsAllYieldSegmentsSent(activeYieldCache))
                {
                    if (await IsYieldCompletionConfirmedAsync(robot, activeYieldCache, ct))
                    {
                        await CleanupYieldLocksOnCompletionAsync(robot, activeYieldCache, ct);
                        await DeleteYieldCacheAsync(activeYieldCacheKey);
                        await _vdaOrderGateService.MarkSessionCompletedAsync(
                            robot.RobotId,
                            VdaOrderChannel.Yield,
                            activeYieldCache.YieldOrderId,
                            activeYieldCache.OrderUpdateId,
                            ct);
                    }
                    else
                    {
                        _logger.LogDebug(
                            "[让行] 让行缓存已全发送但状态未确认完成，继续保持让行优先: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}",
                            request.IdleRobotId,
                            activeYieldCache.YieldOrderId);
                        return new IdleYieldResult
                        {
                            Success = true,
                            OrderId = activeYieldCache.YieldOrderId,
                            TargetNodeCode = activeYieldCache.TargetNodeCode
                        };
                    }
                }
                else
                {
                    var continued = await TryContinueYieldDispatchAsync(
                        robot.RobotManufacturer,
                        robot.RobotSerialNumber,
                        ct);
                    if (continued)
                    {
                        _logger.LogInformation(
                            "[让行] 命中进行中让行任务，继续当前让行: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}",
                            request.IdleRobotId,
                            activeYieldCache.YieldOrderId);
                        return new IdleYieldResult
                        {
                            Success = true,
                            OrderId = activeYieldCache.YieldOrderId,
                            TargetNodeCode = activeYieldCache.TargetNodeCode
                        };
                    }

                    _logger.LogWarning(
                        "[让行] 检测到进行中让行任务但续发未完成，将回退到新建让行流程: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}",
                        request.IdleRobotId,
                        activeYieldCache.YieldOrderId);
                }
            }

            // 3. 检查冷却期（仅新建让行时生效）。
            if (await IsInYieldCooldownAsync(request.IdleRobotId))
            {
                return new IdleYieldResult { Success = false, FailureReason = "Robot in yield cooldown" };
            }

            // 4. 验证机器人确实空闲
            var status = await _robotCacheService.GetStatusAsync(robot.RobotManufacturer, robot.RobotSerialNumber);
            if (status?.Status != RobotStatus.Idle)
            {
                return new IdleYieldResult { Success = false, FailureReason = $"Robot not idle, current status: {status?.Status}" };
            }

            var location = await _robotCacheService.GetLocationAsync(robot.RobotManufacturer, robot.RobotSerialNumber);
            var currentNodeCode = await RobotNavigationNodeResolver.ResolveAsync(
                _agvPathService,
                request.MapId,
                location,
                fallbackLastNodeCode: status?.LastNode,
                ct: ct);
            if (string.IsNullOrWhiteSpace(currentNodeCode))
            {
                return new IdleYieldResult { Success = false, FailureReason = "Unable to resolve idle robot current node" };
            }

            var currentTheta = location?.Theta ?? robot.CurrentTheta ?? 0d;
            var graph = await _agvPathService.GetOrBuildGraphAsync(request.MapId);
            if (graph == null)
            {
                return new IdleYieldResult { Success = false, FailureReason = "Graph not available" };
            }

            var skipYieldResult = await TrySkipYieldWhenSubTaskCanProceedAsync(
                request,
                requestingRobot,
                robot,
                taskSnapshot,
                graph,
                currentNodeCode,
                currentTheta,
                ct);
            if (skipYieldResult != null)
            {
                return skipYieldResult;
            }

            var remainingTaskLockCandidates = NextSegmentLockScopeBuilder.BuildRemainingTaskLockCandidates(
                requestingCache,
                graph,
                requestingRobot);
            if (remainingTaskLockCandidates.Count == 0)
            {
                return new IdleYieldResult { Success = false, FailureReason = "Remaining task lock scope unavailable" };
            }

            // 5. 查找让行目标节点
            var targetNodeCode = request.TargetNodeCode
                ?? await FindNearestYieldTargetAsync(
                    request.MapId,
                    request.MapCode,
                    robot,
                    request.BlockingNodeCode,
                    currentNodeCode,
                    currentTheta,
                    request.IdleRobotId,
                    requestingRobot,
                    requestingCache,
                    ct);
            if (string.IsNullOrWhiteSpace(targetNodeCode))
            {
                return new IdleYieldResult { Success = false, FailureReason = "No available yield target node" };
            }

            var targetNode = graph.Nodes.Values.FirstOrDefault(node =>
                string.Equals(node.NodeCode, targetNodeCode, StringComparison.OrdinalIgnoreCase));
            var forbiddenTargetResource = targetNode == null
                ? null
                : GetYieldForbiddenResource(graph, targetNode.NodeId);
            if (forbiddenTargetResource != null)
            {
                _logger.LogDebug(
                    "[让行] 让行目标位于不允许避让资源区域，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Resource={ResourceCode}",
                    robot.RobotId,
                    targetNodeCode,
                    forbiddenTargetResource.ResourceCode);
                return new IdleYieldResult { Success = false, FailureReason = "Yield target node is in forbidden yield resource" };
            }

            // 6. 规划让行路径
            var pathSegments = await PlanYieldPathAsync(
                robot,
                request.MapId,
                graph,
                currentNodeCode,
                currentTheta,
                targetNodeCode,
                ct);
            if (pathSegments.Count == 0)
            {
                return new IdleYieldResult { Success = false, FailureReason = "No path to target node" };
            }

            // 7. 按主流程规则切割让行路径并先保存缓存
            var orderId = $"YIELD-{Guid.NewGuid():N}";
            var segmentedPath = _vdaPathSegmentationService.SplitSegmentsByBoundary(
                pathSegments,
                graph,
                robot,
                hasTerminalAction: false);
            if (segmentedPath.Count == 0)
            {
                return new IdleYieldResult { Success = false, FailureReason = "Yield path segmentation failed" };
            }

            var cache = BuildYieldSegmentedPathCache(
                robot,
                request.MapId,
                request.MapCode,
                targetNodeCode,
                orderId,
                segmentedPath);
            var cacheKey = BuildYieldCacheKey(robot.RobotId, orderId);
            if (!await SaveYieldCacheAsync(cacheKey, cache, ct))
            {
                return new IdleYieldResult { Success = false, FailureReason = "Yield cache save failed" };
            }

            // 8. 仅下发首段
            if (!TryLocateCurrentYieldResourceSegment(cache, out var currentJunctionIndex, out var currentResourceIndex, out var currentSegment))
            {
                return new IdleYieldResult { Success = false, FailureReason = "Yield segmented cache has no dispatchable segment" };
            }

            var mqttClientService = _serviceProvider.GetRequiredService<IMqttClientService>();
            var gateResult = await _vdaOrderGateService.TryAcquireAsync(
                robot.RobotId,
                VdaOrderChannel.Yield,
                cache.YieldOrderId,
                cache.OrderUpdateId,
                ct);
            if (!gateResult.Success)
            {
                return new IdleYieldResult { Success = false, FailureReason = gateResult.Message };
            }

            Domain.Models.VDA5050.Order order;
            var nextHeaderId = checked((int)(robot.HeaderId + 1));
            var gateLease = gateResult.Lease;
            try
            {
                if (cache.OrderUpdateId > 0 && _settings.Vda5050.EnableOrderUpdatePrecheck)
                {
                    var precheck = await _vdaOrderGateService.ValidateOrderUpdatePrecheckAsync(
                        robot.RobotId,
                        VdaOrderChannel.Yield,
                        robot.RobotManufacturer,
                        robot.RobotSerialNumber,
                        cache.YieldOrderId,
                        cache.OrderUpdateId,
                        ct);
                    if (!precheck.Success)
                    {
                        await _vdaOrderGateService.RecordOrderUpdateDowngradeAsync(
                            robot.RobotId,
                            VdaOrderChannel.Yield,
                            cache.YieldOrderId,
                            cache.OrderUpdateId,
                            precheck.Reason,
                            ct);

                        var downgradedCache = await RebuildYieldCacheAfterPrecheckFailureAsync(
                            robot,
                            cache,
                            ct);
                        if (downgradedCache == null)
                        {
                            return new IdleYieldResult { Success = false, FailureReason = "Yield downgrade rebuild failed" };
                        }

                        cache = downgradedCache;
                        graph = await _agvPathService.GetOrBuildGraphAsync(cache.MapId) ?? graph;
                        if (!TryLocateCurrentYieldResourceSegment(cache, out currentJunctionIndex, out currentResourceIndex, out currentSegment))
                        {
                            return new IdleYieldResult { Success = false, FailureReason = "Yield cache has no dispatchable segment after downgrade" };
                        }
                    }
                }

                var isFinalDispatch = IsFinalYieldResourceSegment(cache, currentJunctionIndex, currentResourceIndex);
                var startSequenceId = ResolveYieldStartSequenceId(cache);
                nextHeaderId = checked((int)(robot.HeaderId + 1));
                order = BuildYieldOrder(
                    robot,
                    cache.YieldOrderId,
                    nextHeaderId,
                    cache.OrderUpdateId,
                    currentSegment.Segments,
                    graph,
                    cache.MapCode,
                    startSequenceId,
                    isFinalDispatch);

                await _vdaOrderGateService.MarkSessionSendingAsync(
                    robot.RobotId,
                    VdaOrderChannel.Yield,
                    order.OrderId,
                    order.OrderUpdateId,
                    null,
                    ct);

                await mqttClientService.PublishOrderAsync(
                    robot.ProtocolName,
                    robot.ProtocolVersion,
                    robot.RobotManufacturer,
                    robot.RobotSerialNumber,
                    order,
                    ct: ct);

                await _vdaOrderGateService.MarkSessionSentAsync(
                    robot.RobotId,
                    VdaOrderChannel.Yield,
                    order.OrderId,
                    order.OrderUpdateId,
                    ct);
                await PersistRobotHeaderIdAsync(robot, nextHeaderId, ct);
                cache.LastSentSequenceId = GetLastReleasedSequenceId(order, startSequenceId);
            }
            catch (Exception ex)
            {
                await _vdaOrderGateService.MarkSessionFailedAsync(
                    robot.RobotId,
                    VdaOrderChannel.Yield,
                    cache.YieldOrderId,
                    cache.OrderUpdateId,
                    ex.Message,
                    ct);
                throw;
            }
            finally
            {
                if (gateLease != null)
                {
                    await gateLease.DisposeAsync();
                }
            }

            var expectedPlanVersion = cache.PlanVersion;
            if (TryMarkYieldResourceSent(cache, currentJunctionIndex, currentResourceIndex))
            {
                if (cache.PlanVersion <= expectedPlanVersion)
                {
                    cache.PlanVersion = expectedPlanVersion + 1;
                }

                if (!await TryPersistYieldCacheCasWithFallbackAsync(cacheKey, cache, expectedPlanVersion, ct))
                {
                    _logger.LogWarning(
                        "[让行] 首段下发后更新让行缓存失败: IdleRobot={IdleRobotId}, CacheKey={CacheKey}",
                        request.IdleRobotId,
                        cacheKey);
                }
            }

            // 9. 设置冷却期
            var cooldownSeconds = _settings.GlobalNavigation?.Coordinator?.IdleYieldCooldownSeconds ?? 30;
            await SetYieldCooldownAsync(request.IdleRobotId, cooldownSeconds);

            _logger.LogInformation(
                "[让行] 已下发让行订单: IdleRobot={IdleRobotId}, From={From}, To={To}, OrderId={OrderId}",
                request.IdleRobotId, currentNodeCode, targetNodeCode, cache.YieldOrderId);

            return new IdleYieldResult
            {
                Success = true,
                OrderId = cache.YieldOrderId,
                TargetNodeCode = targetNodeCode
            };
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "[让行] 执行让行异常: IdleRobot={IdleRobotId}", request.IdleRobotId);
            return new IdleYieldResult { Success = false, FailureReason = ex.Message };
        }
    }

    /// <inheritdoc />
    public async Task<string?> FindNearestYieldTargetAsync(
        Guid mapId,
        string mapCode,
        Robot robot,
        string blockingAnchorNodeCode,
        string idleCurrentNodeCode,
        double idleCurrentTheta,
        Guid excludeRobotId,
        Robot requestingRobot,
        VdaSegmentedPathCache requestingCache,
        CancellationToken ct = default)
    {
        ct.ThrowIfCancellationRequested();
        var graph = await _agvPathService.GetOrBuildGraphAsync(mapId);
        if (graph == null) return null;

        var idleCurrentNode = graph.Nodes.Values.FirstOrDefault(n =>
            string.Equals(n.NodeCode, idleCurrentNodeCode, StringComparison.OrdinalIgnoreCase));
        if (idleCurrentNode == null) return null;

        var remainingTaskLockCandidates = NextSegmentLockScopeBuilder.BuildRemainingTaskLockCandidates(
            requestingCache,
            graph,
            requestingRobot);
        if (remainingTaskLockCandidates.Count == 0)
        {
            _logger.LogDebug(
                "[让行] 请求车剩余任务扫掠锁候选为空: RequestingRobot={RequestingRobotId}",
                requestingRobot.RobotId);
            return null;
        }

        var remainingTaskResourceKeys = NextSegmentLockScopeBuilder.BuildResourceKeys(remainingTaskLockCandidates);
        if (remainingTaskResourceKeys.Count == 0)
        {
            _logger.LogDebug(
                "[让行] 请求车剩余任务扫掠资源键为空: RequestingRobot={RequestingRobotId}",
                requestingRobot.RobotId);
            return null;
        }

        var remainingTaskSweptZones = NextSegmentLockScopeBuilder.BuildRemainingTaskSweptZones(
            requestingCache,
            graph,
            requestingRobot);
        if (remainingTaskSweptZones.Count == 0)
        {
            _logger.LogDebug(
                "[让行] 请求车剩余任务连续扫掠区为空: RequestingRobot={RequestingRobotId}",
                requestingRobot.RobotId);
            return null;
        }

        var orderedCandidates = BuildDivergentYieldCandidates(
            graph,
            idleCurrentNode,
            idleCurrentNodeCode,
            blockingAnchorNodeCode);
        if (orderedCandidates.Count == 0)
        {
            return null;
        }

        foreach (var candidate in orderedCandidates)
        {
            ct.ThrowIfCancellationRequested();

            if (await _trafficControl.IsNodeOccupiedByOtherAsync(mapCode, candidate.Node.NodeCode, excludeRobotId))
            {
                _logger.LogDebug(
                    "[让行] 候选目标被占用，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}",
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Hop);
                continue;
            }

            var parkingLockCandidates = NextSegmentLockScopeBuilder.BuildNodeParkingLockCandidates(
                candidate.Node.NodeCode,
                graph,
                robot);
            if (parkingLockCandidates.Count == 0)
            {
                _logger.LogDebug(
                    "[让行] 无法构建目标驻车扫掠锁候选，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}",
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Hop);
                continue;
            }

            var parkingResourceKeys = NextSegmentLockScopeBuilder.BuildResourceKeys(parkingLockCandidates);
            if (parkingResourceKeys.Overlaps(remainingTaskResourceKeys))
            {
                var overlapResources = parkingResourceKeys
                    .Where(remainingTaskResourceKeys.Contains)
                    .Take(8)
                    .ToList();
                _logger.LogDebug(
                    "[让行] 候选目标驻车扫掠资源与任务车剩余任务资源交集命中，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}, OverlapResources={OverlapResources}",
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Hop,
                    overlapResources);
                continue;
            }

            var parkingSweptZones = NextSegmentLockScopeBuilder.BuildNodeParkingSweptZones(
                candidate.Node.NodeCode,
                graph,
                robot);
            if (parkingSweptZones.Count == 0)
            {
                _logger.LogDebug(
                    "[让行] 无法构建目标驻车连续扫掠区，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}",
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Hop);
                continue;
            }

            var geometricIntersection = SweptAreaCoverageResolver.ZonesIntersect(
                parkingSweptZones,
                remainingTaskSweptZones);
            if (geometricIntersection)
            {
                _logger.LogDebug(
                    "[让行] 候选与任务车剩余扫掠区存在几何交集，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}",
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Hop);
                continue;
            }

            var plannedPath = await PlanYieldPathAsync(
                robot,
                mapId,
                graph,
                idleCurrentNodeCode,
                idleCurrentTheta,
                candidate.Node.NodeCode,
                ct);
            if (plannedPath.Count == 0)
            {
                _logger.LogDebug(
                    "[让行] 候选目标不可达，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}",
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Hop);
                continue;
            }

            var pathSweptLockCandidates = NextSegmentLockScopeBuilder.BuildSweptLockCandidates(
                plannedPath,
                graph,
                robot);
            var sweptNodeCodes = DistinctCodesInOrder(pathSweptLockCandidates
                .SelectMany(segment => new[] { segment.FromNodeCode, segment.ToNodeCode }));
            var sweptEdgeCodes = DistinctCodesInOrder(pathSweptLockCandidates
                .Select(segment => segment.EdgeCode));
            var sweptEdgeDirectionMap = BuildEdgeDirectionMap(pathSweptLockCandidates);

            if (await HasAnyExternalLocksAsync(
                    mapCode,
                    sweptNodeCodes,
                    sweptEdgeCodes,
                    sweptEdgeDirectionMap,
                    excludeRobotId,
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Hop,
                    ct))
            {
                continue;
            }

            var forbiddenResource = GetYieldForbiddenResource(graph, candidate.Node.NodeId);
            if (forbiddenResource != null)
            {
                _logger.LogDebug(
                    "[让行] 候选目标位于不允许避让资源区域，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}, Resource={ResourceCode}",
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Hop,
                    forbiddenResource.ResourceCode);
                continue;
            }

            _logger.LogInformation(
                "[让行] 选中零资源键交集且零几何交集让行目标: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}, Distance={Distance}",
                robot.RobotId,
                candidate.Node.NodeCode,
                candidate.Hop,
                candidate.DistanceSquared);
            return candidate.Node.NodeCode;
        }

        _logger.LogInformation(
            "[让行] 发散搜索结束，未找到满足零资源键交集且零几何交集的可用让行目标: IdleRobot={IdleRobotId}",
            robot.RobotId);
        return null;
    }

    /// <inheritdoc />
    public async Task<bool> IsInYieldCooldownAsync(Guid robotId)
    {
        try
        {
            var db = _redis.GetDatabase();
            var key = $"{YieldCooldownKeyPrefix}:{robotId:N}";
            return await db.KeyExistsAsync(key);
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "[让行] 检查冷却期异常: RobotId={RobotId}", robotId);
            return false;
        }
    }

    /// <inheritdoc />
    public async Task SetYieldCooldownAsync(Guid robotId, int cooldownSeconds)
    {
        try
        {
            var db = _redis.GetDatabase();
            var key = $"{YieldCooldownKeyPrefix}:{robotId:N}";
            await db.StringSetAsync(key, "1", TimeSpan.FromSeconds(cooldownSeconds));
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "[让行] 设置冷却期异常: RobotId={RobotId}", robotId);
        }
    }

    /// <inheritdoc />
    public async Task<bool> TryContinueYieldDispatchAsync(
        string robotManufacturer,
        string robotSerialNumber,
        CancellationToken ct = default)
    {
        var hasActiveYield = false;

        try
        {
            var robot = await _robotRepository.GetByManufacturerAndSerialNumberAsync(
                robotManufacturer,
                robotSerialNumber,
                ct) as Robot;
            if (robot == null || !robot.Active)
            {
                return false;
            }

            var (cacheKey, cache) = await GetLatestYieldCacheAsync(robot.RobotId, ct);
            if (cache == null || string.IsNullOrWhiteSpace(cacheKey))
            {
                return false;
            }

            hasActiveYield = true;
            if (IsAllYieldSegmentsSent(cache))
            {
                if (await IsYieldCompletionConfirmedAsync(robot, cache, ct))
                {
                    await CleanupYieldLocksOnCompletionAsync(robot, cache, ct);
                    await DeleteYieldCacheAsync(cacheKey);
                    await _vdaOrderGateService.MarkSessionCompletedAsync(
                        robot.RobotId,
                        VdaOrderChannel.Yield,
                        cache.YieldOrderId,
                        cache.OrderUpdateId,
                        ct);
                    return false;
                }

                return true;
            }

            if (!await IsReadyForNextYieldDispatchAsync(robot, cache, ct))
            {
                _logger.LogDebug(
                    "[让行] 让行缓存存在但尚未到续发时机，保持让行优先: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}",
                    robot.RobotId,
                    cache.YieldOrderId);
                return true;
            }

            if (!TryLocateCurrentYieldResourceSegment(cache, out var junctionIndex, out var resourceIndex, out var segmentItem))
            {
                await DeleteYieldCacheAsync(cacheKey);
                return false;
            }

            var graph = await _agvPathService.GetOrBuildGraphAsync(cache.MapId);
            if (graph == null)
            {
                _logger.LogWarning(
                    "[让行] 让行续发失败，地图图结构不可用: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}",
                    robot.RobotId,
                    cache.YieldOrderId);
                return true;
            }

            var mqttClientService = _serviceProvider.GetRequiredService<IMqttClientService>();
            var gateResult = await _vdaOrderGateService.TryAcquireAsync(
                robot.RobotId,
                VdaOrderChannel.Yield,
                cache.YieldOrderId,
                cache.OrderUpdateId,
                ct);
            if (!gateResult.Success)
            {
                _logger.LogDebug(
                    "[让行] 让行续发门闩阻断: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}, Message={Message}",
                    robot.RobotId,
                    cache.YieldOrderId,
                    gateResult.Message);
                return true;
            }

            Domain.Models.VDA5050.Order order;
            var nextHeaderId = checked((int)(robot.HeaderId + 1));
            var gateLease = gateResult.Lease;
            try
            {
                if (cache.OrderUpdateId > 0 && _settings.Vda5050.EnableOrderUpdatePrecheck)
                {
                    var precheck = await _vdaOrderGateService.ValidateOrderUpdatePrecheckAsync(
                        robot.RobotId,
                        VdaOrderChannel.Yield,
                        robot.RobotManufacturer,
                        robot.RobotSerialNumber,
                        cache.YieldOrderId,
                        cache.OrderUpdateId,
                        ct);
                    if (!precheck.Success)
                    {
                        await _vdaOrderGateService.RecordOrderUpdateDowngradeAsync(
                            robot.RobotId,
                            VdaOrderChannel.Yield,
                            cache.YieldOrderId,
                            cache.OrderUpdateId,
                            precheck.Reason,
                            ct);

                        var downgradedCache = await RebuildYieldCacheAfterPrecheckFailureAsync(
                            robot,
                            cache,
                            ct);
                        if (downgradedCache == null)
                        {
                            _logger.LogWarning(
                                "[让行] 让行续发降级重建失败: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}",
                                robot.RobotId,
                                cache.YieldOrderId);
                            return true;
                        }

                        cache = downgradedCache;
                        if (!TryLocateCurrentYieldResourceSegment(cache, out junctionIndex, out resourceIndex, out segmentItem))
                        {
                            _logger.LogWarning(
                                "[让行] 让行续发降级后无可发送分段: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}",
                                robot.RobotId,
                                cache.YieldOrderId);
                            return true;
                        }

                        graph = await _agvPathService.GetOrBuildGraphAsync(cache.MapId) ?? graph;
                    }
                }

                var isFinalDispatch = IsFinalYieldResourceSegment(cache, junctionIndex, resourceIndex);
                var startSequenceId = ResolveYieldStartSequenceId(cache);
                nextHeaderId = checked((int)(robot.HeaderId + 1));
                order = BuildYieldOrder(
                    robot,
                    cache.YieldOrderId,
                    nextHeaderId,
                    cache.OrderUpdateId,
                    segmentItem.Segments,
                    graph,
                    cache.MapCode,
                    startSequenceId,
                    isFinalDispatch);

                await _vdaOrderGateService.MarkSessionSendingAsync(
                    robot.RobotId,
                    VdaOrderChannel.Yield,
                    order.OrderId,
                    order.OrderUpdateId,
                    null,
                    ct);

                await mqttClientService.PublishOrderAsync(
                    robot.ProtocolName,
                    robot.ProtocolVersion,
                    robot.RobotManufacturer,
                    robot.RobotSerialNumber,
                    order,
                    ct: ct);

                await _vdaOrderGateService.MarkSessionSentAsync(
                    robot.RobotId,
                    VdaOrderChannel.Yield,
                    order.OrderId,
                    order.OrderUpdateId,
                    ct);
                await PersistRobotHeaderIdAsync(robot, nextHeaderId, ct);
                cache.LastSentSequenceId = GetLastReleasedSequenceId(order, startSequenceId);
            }
            catch (Exception ex)
            {
                await _vdaOrderGateService.MarkSessionFailedAsync(
                    robot.RobotId,
                    VdaOrderChannel.Yield,
                    cache.YieldOrderId,
                    cache.OrderUpdateId,
                    ex.Message,
                    ct);
                throw;
            }
            finally
            {
                if (gateLease != null)
                {
                    await gateLease.DisposeAsync();
                }
            }

            var expectedPlanVersion = cache.PlanVersion;
            if (TryMarkYieldResourceSent(cache, junctionIndex, resourceIndex))
            {
                if (cache.PlanVersion <= expectedPlanVersion)
                {
                    cache.PlanVersion = expectedPlanVersion + 1;
                }

                if (!await TryPersistYieldCacheCasWithFallbackAsync(cacheKey, cache, expectedPlanVersion, ct))
                {
                    _logger.LogWarning(
                        "[让行] 让行续发后缓存CAS更新失败: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}, ExpectedPlanVersion={PlanVersion}",
                        robot.RobotId,
                        cache.YieldOrderId,
                        expectedPlanVersion);
                }
            }

            if (IsAllYieldSegmentsSent(cache))
            {
                if (await IsYieldCompletionConfirmedAsync(robot, cache, ct))
                {
                    await CleanupYieldLocksOnCompletionAsync(robot, cache, ct);
                    await DeleteYieldCacheAsync(cacheKey);
                    await _vdaOrderGateService.MarkSessionCompletedAsync(
                        robot.RobotId,
                        VdaOrderChannel.Yield,
                        cache.YieldOrderId,
                        cache.OrderUpdateId,
                        ct);
                }
            }

            _logger.LogInformation(
                "[让行] 已续发让行分段: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}, OrderUpdateId={OrderUpdateId}, JunctionIndex={JunctionIndex}, ResourceIndex={ResourceIndex}",
                robot.RobotId,
                cache.YieldOrderId,
                order.OrderUpdateId,
                junctionIndex,
                resourceIndex);
            return true;
        }
        catch (Exception ex)
        {
            _logger.LogWarning(
                ex,
                "[让行] 续发让行分段异常: Manufacturer={Manufacturer}, SerialNumber={SerialNumber}",
                robotManufacturer,
                robotSerialNumber);
            return hasActiveYield;
        }
    }

    private async Task<(string? CacheKey, YieldSegmentedPathCache? Cache)> GetLatestYieldCacheAsync(
        Guid robotId,
        CancellationToken ct)
    {
        var endPoints = _redis.GetEndPoints();
        if (endPoints.Length == 0)
        {
            return (null, null);
        }

        var server = _redis.GetServer(endPoints.First());
        var db = _redis.GetDatabase();
        var pattern = $"{_settings.Redis.KeyPrefixes.VdaPath}:{robotId}:{YieldKeyMarker}:*";

        string? latestKey = null;
        YieldSegmentedPathCache? latestCache = null;

        foreach (var key in server.Keys(pattern: pattern))
        {
            ct.ThrowIfCancellationRequested();
            var keyText = key.ToString();
            if (!IsYieldCachePayloadKey(keyText, robotId))
            {
                continue;
            }

            var cacheValue = await db.StringGetAsync(key);
            if (cacheValue.IsNullOrEmpty)
            {
                continue;
            }

            YieldSegmentedPathCache? candidate;
            try
            {
                candidate = JsonSerializer.Deserialize<YieldSegmentedPathCache>(cacheValue.ToString());
            }
            catch (Exception ex) when (ex is JsonException or NotSupportedException)
            {
                _logger.LogDebug(ex, "[让行] 跳过无法反序列化的让行缓存Key: {CacheKey}", keyText);
                continue;
            }

            if (candidate == null)
            {
                continue;
            }

            if (latestCache == null || candidate.CreatedAt > latestCache.CreatedAt)
            {
                latestKey = keyText;
                latestCache = candidate;
            }
        }

        return (latestKey, latestCache);
    }

    private bool IsAllYieldSegmentsSent(YieldSegmentedPathCache cache)
    {
        return !cache.JunctionSegments
            .SelectMany(junction => junction.ResourceSegments)
            .Any(resource => !resource.IsSent);
    }

    private async Task<YieldSegmentedPathCache?> RebuildYieldCacheAfterPrecheckFailureAsync(
        Robot robot,
        YieldSegmentedPathCache cache,
        CancellationToken ct)
    {
        var graph = await _agvPathService.GetOrBuildGraphAsync(cache.MapId);
        if (graph == null)
        {
            return null;
        }

        var status = await _robotCacheService.GetStatusAsync(robot.RobotManufacturer, robot.RobotSerialNumber);
        var location = await _robotCacheService.GetLocationAsync(robot.RobotManufacturer, robot.RobotSerialNumber);
        var currentNodeCode = await RobotNavigationNodeResolver.ResolveAsync(
            _agvPathService,
            cache.MapId,
            location,
            fallbackLastNodeCode: status?.LastNode,
            ct: ct);

        var currentTheta = location?.Theta ?? robot.CurrentTheta ?? 0d;
        var remainingSegments = CollectRemainingYieldSegments(cache);
        List<PathSegmentWithCode> plannedSegments = new();

        if (!string.IsNullOrWhiteSpace(currentNodeCode) &&
            !string.IsNullOrWhiteSpace(cache.TargetNodeCode) &&
            !string.Equals(currentNodeCode, cache.TargetNodeCode, StringComparison.OrdinalIgnoreCase))
        {
            plannedSegments = await PlanYieldPathAsync(
                robot,
                cache.MapId,
                graph,
                currentNodeCode,
                currentTheta,
                cache.TargetNodeCode,
                ct);
        }

        if (plannedSegments.Count == 0)
        {
            plannedSegments = remainingSegments;
        }

        if (plannedSegments.Count == 0)
        {
            return null;
        }

        var segmentedPath = _vdaPathSegmentationService.SplitSegmentsByBoundary(
            plannedSegments,
            graph,
            robot,
            hasTerminalAction: false);
        if (segmentedPath.Count == 0)
        {
            return null;
        }

        return BuildYieldSegmentedPathCache(
            robot,
            cache.MapId,
            cache.MapCode,
            cache.TargetNodeCode,
            $"YIELD-{Guid.NewGuid():N}",
            segmentedPath);
    }

    private static List<PathSegmentWithCode> CollectRemainingYieldSegments(YieldSegmentedPathCache cache)
    {
        var remaining = new List<PathSegmentWithCode>();
        foreach (var junction in cache.JunctionSegments)
        {
            foreach (var resource in junction.ResourceSegments)
            {
                if (resource.IsSent || resource.Segments.Count == 0)
                {
                    continue;
                }

                remaining.AddRange(resource.Segments);
            }
        }

        return remaining;
    }

    private async Task<bool> IsYieldCompletionConfirmedAsync(
        Robot robot,
        YieldSegmentedPathCache cache,
        CancellationToken ct)
    {
        if (!IsAllYieldSegmentsSent(cache))
        {
            return false;
        }

        var status = await _robotCacheService.GetStatusAsync(robot.RobotManufacturer, robot.RobotSerialNumber);
        if (status == null)
        {
            return false;
        }

        if (status.Driving)
        {
            return false;
        }

        if (IsNodeCodeMatchWithVirtualSuffix(status.LastNode, cache.TargetNodeCode))
        {
            return true;
        }

        if (string.Equals(status.CurrentOrderId, cache.YieldOrderId, StringComparison.OrdinalIgnoreCase))
        {
            return (status.CurrentOrderUpdateId ?? 0) >= (uint)Math.Max(0, cache.OrderUpdateId);
        }

        return false;
    }

    private async Task CleanupYieldLocksOnCompletionAsync(
        Robot robot,
        YieldSegmentedPathCache cache,
        CancellationToken ct)
    {
        try
        {
            var status = await _robotCacheService.GetStatusAsync(robot.RobotManufacturer, robot.RobotSerialNumber);
            var releasedCount = await _trafficControl.ReleaseAllRobotLocksAsync(
                robot.RobotId,
                cache.MapCode,
                status?.LastNode);

            _logger.LogInformation(
                "[让行] 让行完成后已清理机器人资源锁: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}, ReleasedCount={ReleasedCount}, KeepNode={KeepNode}",
                robot.RobotId,
                cache.YieldOrderId,
                releasedCount,
                status?.LastNode);
        }
        catch (Exception ex)
        {
            _logger.LogWarning(
                ex,
                "[让行] 让行完成后清理资源锁异常: IdleRobot={IdleRobotId}, YieldOrderId={YieldOrderId}",
                robot.RobotId,
                cache.YieldOrderId);
        }
    }

    private async Task<bool> IsReadyForNextYieldDispatchAsync(
        Robot robot,
        YieldSegmentedPathCache cache,
        CancellationToken ct)
    {
        if (!TryGetLastSentYieldSegment(cache, out var lastSentSegment))
        {
            return true;
        }

        var status = await _robotCacheService.GetStatusAsync(
            robot.RobotManufacturer,
            robot.RobotSerialNumber);
        var lastNode = status?.LastNode;
        if (string.IsNullOrWhiteSpace(lastNode))
        {
            return false;
        }

        return IsNodeCodeMatchWithVirtualSuffix(lastNode, lastSentSegment.ToNodeCode) ||
               IsNodeCodeMatchWithVirtualSuffix(lastNode, lastSentSegment.FromNodeCode);
    }

    private static bool TryGetLastSentYieldSegment(
        YieldSegmentedPathCache cache,
        out PathSegmentWithCode lastSegment)
    {
        for (var j = cache.JunctionSegments.Count - 1; j >= 0; j--)
        {
            var junction = cache.JunctionSegments[j];
            for (var r = junction.ResourceSegments.Count - 1; r >= 0; r--)
            {
                var resource = junction.ResourceSegments[r];
                if (!resource.IsSent || resource.Segments.Count == 0)
                {
                    continue;
                }

                lastSegment = resource.Segments[^1];
                return true;
            }
        }

        lastSegment = null!;
        return false;
    }

    private static bool TryLocateCurrentYieldResourceSegment(
        YieldSegmentedPathCache cache,
        out int junctionIndex,
        out int resourceIndex,
        out VdaSegmentCacheItem segmentItem)
    {
        var jStart = Math.Max(0, cache.CurrentJunctionIndex);
        for (var j = jStart; j < cache.JunctionSegments.Count; j++)
        {
            var junction = cache.JunctionSegments[j];
            var rStart = j == jStart ? Math.Max(0, cache.CurrentResourceIndex) : 0;
            for (var r = rStart; r < junction.ResourceSegments.Count; r++)
            {
                var candidate = junction.ResourceSegments[r];
                if (candidate.IsSent || candidate.Segments.Count == 0)
                {
                    continue;
                }

                cache.CurrentJunctionIndex = j;
                cache.CurrentResourceIndex = r;
                junctionIndex = j;
                resourceIndex = r;
                segmentItem = candidate;
                return true;
            }
        }

        junctionIndex = -1;
        resourceIndex = -1;
        segmentItem = null!;
        return false;
    }

    private static bool IsFinalYieldResourceSegment(
        YieldSegmentedPathCache cache,
        int currentJunctionIndex,
        int currentResourceIndex)
    {
        for (var j = currentJunctionIndex; j < cache.JunctionSegments.Count; j++)
        {
            var junction = cache.JunctionSegments[j];
            var rStart = j == currentJunctionIndex ? currentResourceIndex + 1 : 0;
            for (var r = rStart; r < junction.ResourceSegments.Count; r++)
            {
                var candidate = junction.ResourceSegments[r];
                if (!candidate.IsSent && candidate.Segments.Count > 0)
                {
                    return false;
                }
            }
        }

        return true;
    }

    private static bool TryMarkYieldResourceSent(
        YieldSegmentedPathCache cache,
        int currentJunctionIndex,
        int currentResourceIndex)
    {
        if (currentJunctionIndex < 0 || currentJunctionIndex >= cache.JunctionSegments.Count)
        {
            return false;
        }

        var junction = cache.JunctionSegments[currentJunctionIndex];
        if (currentResourceIndex < 0 || currentResourceIndex >= junction.ResourceSegments.Count)
        {
            return false;
        }

        var current = junction.ResourceSegments[currentResourceIndex];
        current.SendStatus = SegmentSendStatus.Sent;
        current.IsSent = true;

        var nextJunctionIndex = currentJunctionIndex;
        var nextResourceIndex = currentResourceIndex + 1;
        if (nextResourceIndex >= junction.ResourceSegments.Count)
        {
            nextJunctionIndex++;
            nextResourceIndex = 0;
        }

        cache.CurrentJunctionIndex = nextJunctionIndex;
        cache.CurrentResourceIndex = nextResourceIndex;
        cache.OrderUpdateId = Math.Max(0, cache.OrderUpdateId) + 1;
        return true;
    }

    private YieldSegmentedPathCache BuildYieldSegmentedPathCache(
        Robot robot,
        Guid mapId,
        string mapCode,
        string targetNodeCode,
        string orderId,
        List<List<List<PathSegmentWithCode>>> segmentedPath)
    {
        var junctionSegments = segmentedPath
            .Select(junctionGroup => new VdaJunctionSegmentCache
            {
                ResourceSegments = junctionGroup.Select(resourceGroup => new VdaSegmentCacheItem
                {
                    SendStatus = SegmentSendStatus.Pending,
                    IsSent = false,
                    Segments = resourceGroup
                }).ToList()
            })
            .ToList();

        return new YieldSegmentedPathCache
        {
            YieldOrderId = orderId,
            RobotId = robot.RobotId,
            MapId = mapId,
            MapCode = mapCode,
            TargetNodeCode = targetNodeCode,
            CreatedAt = DateTime.Now,
            CurrentJunctionIndex = 0,
            CurrentResourceIndex = 0,
            OrderUpdateId = 0,
            LastSentSequenceId = 0,
            PlanVersion = 1,
            JunctionSegments = junctionSegments
        };
    }

    private async Task<bool> SaveYieldCacheAsync(
        string cacheKey,
        YieldSegmentedPathCache cache,
        CancellationToken ct)
    {
        ct.ThrowIfCancellationRequested();
        var db = _redis.GetDatabase();
        var payload = JsonSerializer.Serialize(cache);
        var tx = db.CreateTransaction();
        _ = tx.StringSetAsync(cacheKey, payload);
        _ = tx.StringSetAsync(BuildYieldPlanVersionKey(cacheKey), cache.PlanVersion.ToString());
        return await tx.ExecuteAsync();
    }

    private async Task<bool> TryPersistYieldCacheCasWithFallbackAsync(
        string cacheKey,
        YieldSegmentedPathCache cache,
        long expectedPlanVersion,
        CancellationToken ct)
    {
        var db = _redis.GetDatabase();
        if (await TryPersistYieldCacheCasAsync(db, cacheKey, cache, expectedPlanVersion, ct))
        {
            return true;
        }

        var latestVersionValue = await db.StringGetAsync(BuildYieldPlanVersionKey(cacheKey));
        if (latestVersionValue.IsNullOrEmpty || !long.TryParse(latestVersionValue.ToString(), out var latestVersion))
        {
            return false;
        }

        if (cache.PlanVersion <= latestVersion)
        {
            cache.PlanVersion = latestVersion + 1;
        }

        return await TryPersistYieldCacheCasAsync(db, cacheKey, cache, latestVersion, ct);
    }

    private static async Task<bool> TryPersistYieldCacheCasAsync(
        IDatabase db,
        string cacheKey,
        YieldSegmentedPathCache cache,
        long expectedPlanVersion,
        CancellationToken ct)
    {
        ct.ThrowIfCancellationRequested();
        var payload = JsonSerializer.Serialize(cache);
        var planVersionKey = BuildYieldPlanVersionKey(cacheKey);

        for (var attempt = 0; attempt < 2; attempt++)
        {
            var tx = db.CreateTransaction();
            tx.AddCondition(Condition.StringEqual(planVersionKey, expectedPlanVersion.ToString()));
            _ = tx.StringSetAsync(cacheKey, payload);
            _ = tx.StringSetAsync(planVersionKey, cache.PlanVersion.ToString());
            if (await tx.ExecuteAsync())
            {
                return true;
            }

            if (attempt == 0)
            {
                await db.StringSetAsync(planVersionKey, expectedPlanVersion.ToString(), when: When.NotExists);
            }
        }

        return false;
    }

    private async Task DeleteYieldCacheAsync(string cacheKey)
    {
        var db = _redis.GetDatabase();
        await db.KeyDeleteAsync(cacheKey);
        await db.KeyDeleteAsync(BuildYieldPlanVersionKey(cacheKey));
    }

    private async Task PersistRobotHeaderIdAsync(Robot robot, int nextHeaderId, CancellationToken ct)
    {
        robot.HeaderId = nextHeaderId;

        var trackedRobot = await _robotRepository.GetByIdAsync(robot.RobotId, ct) as Robot;
        if (trackedRobot == null)
        {
            await _robotRepository.UpdateAsync(robot, ct);
            await _robotRepository.SaveChangesAsync(ct);
            return;
        }

        if (!ReferenceEquals(trackedRobot, robot))
        {
            trackedRobot.HeaderId = nextHeaderId;
        }

        await _robotRepository.SaveChangesAsync(ct);
    }

    private string BuildYieldCacheKey(Guid robotId, string yieldOrderId)
        => $"{_settings.Redis.KeyPrefixes.VdaPath}:{robotId}:{YieldKeyMarker}:{yieldOrderId}";

    private static string BuildYieldPlanVersionKey(string payloadKey) => $"{payloadKey}:planVersion";

    private static bool IsYieldCachePayloadKey(string keyText, Guid robotId)
    {
        if (string.IsNullOrWhiteSpace(keyText) ||
            keyText.EndsWith(":planVersion", StringComparison.OrdinalIgnoreCase))
        {
            return false;
        }

        var marker = $":{robotId}:{YieldKeyMarker}:";
        return keyText.Contains(marker, StringComparison.OrdinalIgnoreCase);
    }

    private static bool IsNodeCodeMatchWithVirtualSuffix(string? actualNodeCode, string? expectedNodeCode)
    {
        if (string.IsNullOrWhiteSpace(actualNodeCode) || string.IsNullOrWhiteSpace(expectedNodeCode))
        {
            return false;
        }

        if (string.Equals(actualNodeCode, expectedNodeCode, StringComparison.OrdinalIgnoreCase))
        {
            return true;
        }

        var normalizedActual = NormalizeVirtualNodeCodeForComparison(actualNodeCode);
        var normalizedExpected = NormalizeVirtualNodeCodeForComparison(expectedNodeCode);
        return string.Equals(normalizedActual, normalizedExpected, StringComparison.OrdinalIgnoreCase);
    }

    private static string NormalizeVirtualNodeCodeForComparison(string nodeCode)
    {
        var trimCode = nodeCode.Trim();
        if (trimCode.Length == 0)
        {
            return trimCode;
        }

        var dashIndex = trimCode.LastIndexOf('-');
        if (dashIndex <= 0 || dashIndex >= trimCode.Length - 1)
        {
            return trimCode;
        }

        var suffix = trimCode[(dashIndex + 1)..];
        for (var i = 0; i < suffix.Length; i++)
        {
            if (!char.IsDigit(suffix[i]))
            {
                return trimCode;
            }
        }

        return trimCode[..dashIndex];
    }

    /// <summary>
    /// 计算两个节点平面距离的平方值。
    /// </summary>
    private static double DistanceSquared(PathNode from, PathNode to)
    {
        var dx = from.X - to.X;
        var dy = from.Y - to.Y;
        return dx * dx + dy * dy;
    }

    /// <summary>
    /// 使用正式寻路服务规划让行路径。
    /// </summary>
    private async Task<List<PathSegmentWithCode>> PlanYieldPathAsync(
        Robot robot,
        Guid mapId,
        PathGraph graph,
        string fromNodeCode,
        double currentTheta,
        string toNodeCode,
        CancellationToken ct)
    {
        if (string.Equals(fromNodeCode, toNodeCode, StringComparison.OrdinalIgnoreCase))
            return new List<PathSegmentWithCode>();

        var start = graph.Nodes.Values.FirstOrDefault(n =>
            string.Equals(n.NodeCode, fromNodeCode, StringComparison.OrdinalIgnoreCase));
        var target = graph.Nodes.Values.FirstOrDefault(n =>
            string.Equals(n.NodeCode, toNodeCode, StringComparison.OrdinalIgnoreCase));
        if (start == null || target == null)
            return new List<PathSegmentWithCode>();

        var request = new PathRequest
        {
            RobotId = robot.RobotId,
            MapId = mapId,
            StartNodeId = start.NodeId,
            EndNodeId = target.NodeId,
            CurrentTheta = currentTheta,
            IsLoaded = false,
            Priority = 0,
            BatteryLevel = robot.BatteryLevel ?? 100,
            MovementType = robot.MovementType,
            ForkRadOffsets = robot.ForkRadOffset,
            RequiredEndRad = target.Theta
        };

        var pathResult = await _agvPathService.CalculatePathAsync(request, ct);
        if (!pathResult.Success || pathResult.Segments.Count == 0)
        {
            return new List<PathSegmentWithCode>();
        }

        return _agvPathService.EnrichSegmentsWithCode(pathResult.Segments, graph);
    }

    private async Task<IdleRobotTaskSnapshot> GetIdleRobotTaskSnapshotAsync(Guid robotId, CancellationToken ct)
    {
        using var scope = _serviceProvider.CreateScope();
        var taskRepository = scope.ServiceProvider.GetRequiredService<IRobotTaskRepository>();
        var robotTasks = (await taskRepository.GetByRobotIdAsync(robotId, ct)).ToList();

        var hasInProgressTaskOrSubTask = robotTasks.Any(task =>
            task.Status == Rcs.Domain.Entities.TaskStatus.InProgress ||
            task.SubTasks.Any(subTask => subTask.Status == Rcs.Domain.Entities.TaskStatus.InProgress));

        var activeTasks = robotTasks
            .Where(task =>
                task.Status == Rcs.Domain.Entities.TaskStatus.Assigned ||
                task.Status == Rcs.Domain.Entities.TaskStatus.InProgress)
            .ToList();

        return new IdleRobotTaskSnapshot(
            hasInProgressTaskOrSubTask,
            activeTasks);
    }

    private async Task<IdleYieldResult?> TrySkipYieldWhenSubTaskCanProceedAsync(
        IdleRobotYieldRequest request,
        Robot requestingRobot,
        Robot idleRobot,
        IdleRobotTaskSnapshot taskSnapshot,
        PathGraph graph,
        string currentNodeCode,
        double currentTheta,
        CancellationToken ct)
    {
        var nextExecutable = await SelectNextExecutableByExecutionQueueRuleAsync(
            idleRobot,
            request.MapId,
            graph,
            currentNodeCode,
            currentTheta,
            taskSnapshot.ActiveTasks,
            ct);
        if (nextExecutable == null)
        {
            return null;
        }

        var task = nextExecutable.Task;
        var nextSubTask = nextExecutable.SubTask;

        if (string.IsNullOrWhiteSpace(request.MapCode))
        {
            _logger.LogDebug(
                "[让行] 地图编码为空，跳过待执行子任务直执判定: IdleRobot={IdleRobotId}, TaskId={TaskId}, SubTaskId={SubTaskId}",
                idleRobot.RobotId,
                task.TaskId,
                nextSubTask.SubTaskId);
            return null;
        }

        if (!graph.Nodes.TryGetValue(nextSubTask.EndNodeId, out var targetNode) ||
            string.IsNullOrWhiteSpace(targetNode.NodeCode))
        {
            _logger.LogDebug(
                "[让行] 待执行子任务终点不在当前地图图结构，回退让行流程: IdleRobot={IdleRobotId}, TaskId={TaskId}, SubTaskId={SubTaskId}, EndNodeId={EndNodeId}",
                idleRobot.RobotId,
                task.TaskId,
                nextSubTask.SubTaskId,
                nextSubTask.EndNodeId);
            return null;
        }

        var targetNodeCode = targetNode.NodeCode;
        if (string.Equals(currentNodeCode, targetNodeCode, StringComparison.OrdinalIgnoreCase))
        {
            _logger.LogInformation(
                "[让行] 让行车待执行子任务可直接执行（当前位置即目标节点），跳过让行: IdleRobot={IdleRobotId}, TaskId={TaskId}, SubTaskId={SubTaskId}, TargetNode={TargetNodeCode}",
                idleRobot.RobotId,
                task.TaskId,
                nextSubTask.SubTaskId,
                targetNodeCode);
            return new IdleYieldResult
            {
                Success = false,
                FailureReason = "Pending subtask can proceed directly, skip yield"
            };
        }

        var plannedPath = await PlanYieldPathAsync(
            idleRobot,
            request.MapId,
            graph,
            currentNodeCode,
            currentTheta,
            targetNodeCode,
            ct);
        if (plannedPath.Count == 0)
        {
            _logger.LogDebug(
                "[让行] 待执行子任务路径不可达，回退让行流程: IdleRobot={IdleRobotId}, TaskId={TaskId}, SubTaskId={SubTaskId}, TargetNode={TargetNodeCode}",
                idleRobot.RobotId,
                task.TaskId,
                nextSubTask.SubTaskId,
                targetNodeCode);
            return null;
        }

        var sweptLockCandidates = NextSegmentLockScopeBuilder.BuildSweptLockCandidates(
            plannedPath,
            graph,
            idleRobot);
        var sweptNodeCodes = DistinctCodesInOrder(sweptLockCandidates
            .SelectMany(segment => new[] { segment.FromNodeCode, segment.ToNodeCode }));
        var sweptEdgeCodes = DistinctCodesInOrder(sweptLockCandidates
            .Select(segment => segment.EdgeCode));
        var sweptEdgeDirectionMap = BuildEdgeDirectionMap(sweptLockCandidates);

        var hasExternalLocks = await HasAnyExternalLocksAsync(
            request.MapCode,
            sweptNodeCodes,
            sweptEdgeCodes,
            sweptEdgeDirectionMap,
            requestingRobot.RobotId,
            idleRobot.RobotId,
            targetNodeCode,
            targetHop: -1,
            ct);
        if (hasExternalLocks)
        {
            return null;
        }

        _logger.LogInformation(
            "[让行] 让行车待执行子任务路径与外部资源锁无冲突，跳过让行: IdleRobot={IdleRobotId}, TaskId={TaskId}, SubTaskId={SubTaskId}, TargetNode={TargetNodeCode}",
            idleRobot.RobotId,
            task.TaskId,
            nextSubTask.SubTaskId,
            targetNodeCode);
        return new IdleYieldResult
        {
            Success = false,
            FailureReason = "Pending subtask can proceed directly, skip yield"
        };
    }

    private async Task<ExecutableSubTaskCandidate?> SelectNextExecutableByExecutionQueueRuleAsync(
        Robot robot,
        Guid mapId,
        PathGraph graph,
        string currentNodeCode,
        double currentTheta,
        IReadOnlyList<RobotTask> activeTasks,
        CancellationToken ct)
    {
        if (activeTasks.Count == 0)
        {
            return null;
        }

        var candidates = new List<ExecutableSubTaskCandidate>();
        foreach (var task in activeTasks)
        {
            ct.ThrowIfCancellationRequested();

            if (!task.SubTasks.Any() && task.Status == Rcs.Domain.Entities.TaskStatus.InProgress)
            {
                continue;
            }

            // 与 TaskExecutionBackgroundService.BuildExecutionQueueAsync 保持一致：复用领域方法判定下一条可执行子任务。
            var nextSubTask = task.GetNextExecutableSubTask();
            if (task.SubTasks.Any() && nextSubTask == null)
            {
                continue;
            }

            if (nextSubTask == null)
            {
                continue;
            }

            var pathCost = await CalculateExecutionQueueLikePathCostAsync(
                robot,
                mapId,
                graph,
                currentNodeCode,
                currentTheta,
                task,
                nextSubTask,
                ct);
            if (pathCost == double.MaxValue || double.IsNaN(pathCost) || double.IsInfinity(pathCost))
            {
                continue;
            }

            candidates.Add(new ExecutableSubTaskCandidate(task, nextSubTask, pathCost));
        }

        return candidates
            .OrderByDescending(candidate => candidate.Task.Priority)
            .ThenBy(candidate => candidate.PathCost)
            .ThenBy(candidate => candidate.CreatedAtMilliseconds)
            .ThenBy(candidate => candidate.Task.TaskId)
            .FirstOrDefault();
    }

    private async Task<double> CalculateExecutionQueueLikePathCostAsync(
        Robot robot,
        Guid mapId,
        PathGraph graph,
        string currentNodeCode,
        double currentTheta,
        RobotTask task,
        RobotSubTask subTask,
        CancellationToken ct)
    {
        if (!graph.Nodes.TryGetValue(subTask.EndNodeId, out var targetNode))
        {
            return double.MaxValue;
        }

        var startNode = graph.Nodes.Values.FirstOrDefault(node =>
            string.Equals(node.NodeCode, currentNodeCode, StringComparison.OrdinalIgnoreCase));
        if (startNode == null)
        {
            return double.MaxValue;
        }

        if (startNode.NodeId == targetNode.NodeId)
        {
            return 0d;
        }

        var request = new PathRequest
        {
            RobotId = robot.RobotId,
            MapId = mapId,
            StartNodeId = startNode.NodeId,
            EndNodeId = targetNode.NodeId,
            CurrentTheta = currentTheta,
            IsLoaded = !string.IsNullOrWhiteSpace(task.ContainerID),
            Priority = task.Priority,
            BatteryLevel = robot.BatteryLevel ?? 100,
            MovementType = robot.MovementType,
            ForkRadOffsets = robot.ForkRadOffset,
            RequiredEndRad = targetNode.Theta
        };

        try
        {
            var pathResult = await _agvPathService.CalculatePathAsync(request, ct);
            if (!pathResult.Success || double.IsNaN(pathResult.TotalCost) || double.IsInfinity(pathResult.TotalCost))
            {
                return double.MaxValue;
            }

            return pathResult.TotalCost;
        }
        catch
        {
            return double.MaxValue;
        }
    }

    private static List<YieldTargetCandidate> BuildDivergentYieldCandidates(
        PathGraph graph,
        PathNode idleCurrentNode,
        string idleCurrentNodeCode,
        string blockingAnchorNodeCode)
    {
        var visited = new HashSet<Guid> { idleCurrentNode.NodeId };
        var hopByNodeId = new Dictionary<Guid, int> { [idleCurrentNode.NodeId] = 0 };
        var queue = new Queue<Guid>();
        queue.Enqueue(idleCurrentNode.NodeId);

        while (queue.Count > 0)
        {
            var currentId = queue.Dequeue();
            if (!graph.Nodes.TryGetValue(currentId, out var currentNode))
            {
                continue;
            }

            var currentHop = hopByNodeId[currentId];
            var neighborIds = currentNode.OutEdges
                .Select(edge => edge.ToNodeId)
                .Concat(currentNode.InEdges.Select(edge => edge.FromNodeId))
                .Distinct();

            foreach (var neighborId in neighborIds)
            {
                if (!graph.Nodes.ContainsKey(neighborId) || !visited.Add(neighborId))
                {
                    continue;
                }

                hopByNodeId[neighborId] = currentHop + 1;
                queue.Enqueue(neighborId);
            }
        }

        return graph.Nodes.Values
            .Where(node => node.Active && !string.IsNullOrWhiteSpace(node.NodeCode))
            .Where(node => !string.Equals(node.NodeCode, idleCurrentNodeCode, StringComparison.OrdinalIgnoreCase))
            .Where(node => !string.Equals(node.NodeCode, blockingAnchorNodeCode, StringComparison.OrdinalIgnoreCase))
            .Where(node => hopByNodeId.ContainsKey(node.NodeId))
            .Select(node => new YieldTargetCandidate(
                node,
                hopByNodeId[node.NodeId],
                DistanceSquared(idleCurrentNode, node)))
            .OrderBy(candidate => candidate.Hop)
            .ThenBy(candidate => candidate.DistanceSquared)
            .ThenBy(candidate => candidate.Node.NodeCode, StringComparer.OrdinalIgnoreCase)
            .ToList();
    }

    private static PathResource? GetYieldForbiddenResource(
        PathGraph graph,
        Guid nodeId)
    {
        return graph.Resources.FirstOrDefault(resource =>
            !resource.AllowYield &&
            resource.NodeIds.Contains(nodeId));
    }

    private static List<string> DistinctCodesInOrder(IEnumerable<string?> codes)
    {
        var seen = new HashSet<string>(StringComparer.OrdinalIgnoreCase);
        var result = new List<string>();
        foreach (var code in codes)
        {
            if (string.IsNullOrWhiteSpace(code) || !seen.Add(code))
            {
                continue;
            }

            result.Add(code);
        }

        return result;
    }

    private static Dictionary<string, (string FromNodeCode, string ToNodeCode)> BuildEdgeDirectionMap(
        IEnumerable<PathSegmentWithCode> lockCandidates)
    {
        return lockCandidates
            .Where(segment => !string.IsNullOrWhiteSpace(segment.EdgeCode))
            .GroupBy(segment => segment.EdgeCode!, StringComparer.OrdinalIgnoreCase)
            .ToDictionary(
                group => group.Key,
                group =>
                {
                    var preferred = group.FirstOrDefault(segment =>
                        !string.IsNullOrWhiteSpace(segment.FromNodeCode) &&
                        !string.IsNullOrWhiteSpace(segment.ToNodeCode)) ?? group.First();
                    return (preferred.FromNodeCode, preferred.ToNodeCode);
                },
                StringComparer.OrdinalIgnoreCase);
    }

    private async Task<bool> HasAnyExternalLocksAsync(
        string mapCode,
        IReadOnlyCollection<string> nodeCodes,
        IReadOnlyCollection<string> edgeCodes,
        Dictionary<string, (string FromNodeCode, string ToNodeCode)> edgeDirectionMap,
        Guid selfRobotId,
        Guid idleRobotId,
        string targetNodeCode,
        int targetHop,
        CancellationToken ct)
    {
        ct.ThrowIfCancellationRequested();

        if (nodeCodes.Count == 0 && edgeCodes.Count == 0)
        {
            _logger.LogDebug(
                "[让行] 候选路径扫掠锁范围为空，按严格约束跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}",
                idleRobotId,
                targetNodeCode,
                targetHop);
            return true;
        }

        var conflictResult = await _trafficControl.CheckPathConflictAsync(
            mapCode,
            nodeCodes,
            edgeCodes,
            selfRobotId,
            edgeDirectionMap.Count > 0 ? edgeDirectionMap : null);

        if (conflictResult.Conflicts.Count == 0 && conflictResult.SameDirectionConflicts.Count == 0)
        {
            return false;
        }

        var conflictResources = conflictResult.Conflicts
            .Select(conflict =>
                conflict.ConflictType == ConflictType.NodeOccupied
                    ? $"N:{conflict.NodeCode}"
                    : $"E:{conflict.EdgeCode}")
            .Concat(conflictResult.SameDirectionConflicts.Select(conflict => $"E:{conflict.EdgeCode}"))
            .Where(resource => !string.IsNullOrWhiteSpace(resource))
            .Distinct(StringComparer.OrdinalIgnoreCase)
            .Take(8)
            .ToList();

        var conflictingRobots = conflictResult.Conflicts
            .Concat(conflictResult.SameDirectionConflicts)
            .Select(conflict => conflict.ConflictingRobotId)
            .Distinct()
            .ToList();

        _logger.LogDebug(
            "[让行] 候选路径扫掠区命中外部资源锁，跳过: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Hop={Hop}, HardConflicts={HardConflictCount}, SameDirectionConflicts={SameDirectionConflictCount}, ConflictingRobots={ConflictingRobots}, Resources={Resources}",
            idleRobotId,
            targetNodeCode,
            targetHop,
            conflictResult.Conflicts.Count,
            conflictResult.SameDirectionConflicts.Count,
            conflictingRobots,
            conflictResources);
        return true;
    }

    private sealed record IdleRobotTaskSnapshot(
        bool HasInProgressTaskOrSubTask,
        IReadOnlyList<RobotTask> ActiveTasks);

    private sealed record ExecutableSubTaskCandidate(
        RobotTask Task,
        RobotSubTask SubTask,
        double PathCost)
    {
        public long CreatedAtMilliseconds { get; } = Task.CreatedAt.Ticks / TimeSpan.TicksPerMillisecond;
    }

    /// <summary>
    /// 构建让行 VDA5050 Order。
    /// </summary>
    private static int ResolveYieldStartSequenceId(YieldSegmentedPathCache cache)
    {
        if (cache.OrderUpdateId <= 0)
        {
            return 0;
        }

        if (cache.LastSentSequenceId > 0)
        {
            return cache.LastSentSequenceId;
        }

        var inferred = InferYieldLastSentSequenceId(cache);
        cache.LastSentSequenceId = inferred;
        return inferred;
    }

    private static int InferYieldLastSentSequenceId(YieldSegmentedPathCache cache)
    {
        var sequenceId = 0;
        foreach (var junction in cache.JunctionSegments)
        {
            foreach (var resource in junction.ResourceSegments)
            {
                if (!(resource.IsSent || resource.SendStatus == SegmentSendStatus.Sent))
                {
                    return sequenceId;
                }

                sequenceId += resource.Segments.Count * 2;
            }
        }

        return sequenceId;
    }

    private static int GetLastReleasedSequenceId(Domain.Models.VDA5050.Order order, int startSequenceId)
    {
        var lastReleasedNodeSeq = order.Nodes
            .Where(n => n.Released)
            .Select(n => n.SequenceId)
            .DefaultIfEmpty(startSequenceId)
            .Max();

        var lastReleasedEdgeSeq = order.Edges
            .Where(e => e.Released)
            .Select(e => e.SequenceId)
            .DefaultIfEmpty(startSequenceId)
            .Max();

        return Math.Max(lastReleasedNodeSeq, lastReleasedEdgeSeq);
    }

    private Domain.Models.VDA5050.Order BuildYieldOrder(
        Robot robot,
        string orderId,
        int headerId,
        int orderUpdateId,
        List<PathSegmentWithCode> segments,
        PathGraph graph,
        string mapCode,
        int startSequenceId,
        bool isFinalDispatch)
    {
        var nodes = new List<Node>();
        var edges = new List<Edge>();
        var sequenceId = Math.Max(0, startSequenceId);
        var coordinateScale = robot.CoordinateScale > 0 ? robot.CoordinateScale : 1d;

        for (var i = 0; i < segments.Count; i++)
        {
            var seg = segments[i];

            // 添加起点节点（仅第一段）
            if (i == 0 && graph.Nodes.TryGetValue(seg.FromNodeId, out var fromNode))
            {
                nodes.Add(new Node
                {
                    NodeId = seg.FromNodeCode,
                    SequenceId = sequenceId,
                    Released = true,
                    NodeDescription = "Yield start",
                    NodePosition = new NodePosition
                    {
                        X = fromNode.X / coordinateScale,
                        Y = fromNode.Y / coordinateScale,
                        MapId = mapCode,
                        AllowedDeviationXY = fromNode.MaxCoordinateOffset ?? PositionConstants.AllowedDeviationPosition / coordinateScale
                    },
                    Actions = new List<Domain.Models.VDA5050.Action>()
                });
            }

            // 添加边
            var edgeSequenceId = sequenceId + 1;
            var nodeSequenceId = sequenceId + 2;
            var startNodeCode = nodes.Count > 0 ? nodes[^1].NodeId : seg.FromNodeCode;

            edges.Add(new Edge
            {
                EdgeId = seg.EdgeCode,
                SequenceId = edgeSequenceId,
                Released = true,
                StartNodeId = startNodeCode,
                EndNodeId = seg.ToNodeCode,
                MaxSpeed = seg.MaxSpeed,
                Orientation = seg.Angle
            });

            // 添加终点节点
            if (graph.Nodes.TryGetValue(seg.ToNodeId, out var toNode))
            {
                nodes.Add(new Node
                {
                    NodeId = seg.ToNodeCode,
                    SequenceId = nodeSequenceId,
                    Released = true,
                    NodeDescription = i == segments.Count - 1
                        ? (isFinalDispatch ? "Yield target" : "Yield waypoint")
                        : "Yield waypoint",
                    NodePosition = new NodePosition
                    {
                        X = toNode.X / coordinateScale,
                        Y = toNode.Y / coordinateScale,
                        MapId = mapCode,
                        AllowedDeviationXY = toNode.MaxCoordinateOffset ?? PositionConstants.AllowedDeviationPosition / coordinateScale
                    },
                    Actions = new List<Domain.Models.VDA5050.Action>()
                });
            }

            sequenceId = nodeSequenceId;
        }

        return new Domain.Models.VDA5050.Order
        {
            HeaderId = headerId,
            Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffzzz"),
            Version = robot.ProtocolVersion,
            Manufacturer = robot.RobotManufacturer,
            SerialNumber = robot.RobotSerialNumber,
            ZoneSetId = mapCode,
            OrderId = orderId,
            OrderUpdateId = orderUpdateId,
            Nodes = nodes,
            Edges = edges
        };
    }

    private sealed record YieldTargetCandidate(PathNode Node, int Hop, double DistanceSquared);
}