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.Shared;
using Rcs.Domain.Entities;
using Rcs.Domain.Enums;
using Rcs.Domain.Models.VDA5050;
using Rcs.Domain.Repositories;
using Rcs.Domain.Settings;
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 ILogger<IdleRobotYieldService> _logger;
    private readonly AppSettings _settings;

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

    /// <inheritdoc />
    public async Task<IdleYieldResult> ExecuteYieldAsync(
        IdleRobotYieldRequest request,
        Robot requestingRobot,
        VdaSegmentedPathCache requestingCache,
        CancellationToken ct = default)
    {
        try
        {
            // 1. 检查冷却期
            if (await IsInYieldCooldownAsync(request.IdleRobotId))
            {
                return new IdleYieldResult { Success = false, FailureReason = "Robot in yield cooldown" };
            }

            // 2. 获取空闲机器人信息
            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" };
            }

            // 3. 验证机器人确实空闲
            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 remainingTaskLockCandidates = NextSegmentLockScopeBuilder.BuildRemainingTaskLockCandidates(
                requestingCache,
                graph,
                requestingRobot);
            if (remainingTaskLockCandidates.Count == 0)
            {
                return new IdleYieldResult { Success = false, FailureReason = "Remaining task lock scope unavailable" };
            }

            // 4. 查找让行目标节点
            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" };
            }

            // 5. 规划让行路径
            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" };
            }

            // 6. 按主流程规则切割让行路径并先保存缓存
            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" };
            }

            // 7. 仅下发首段
            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 isFinalDispatch = IsFinalYieldResourceSegment(cache, currentJunctionIndex, currentResourceIndex);
            var nextHeaderId = checked((int)(robot.HeaderId + 1));
            var order = BuildYieldOrder(
                robot,
                orderId,
                nextHeaderId,
                cache.OrderUpdateId,
                currentSegment.Segments,
                graph,
                request.MapCode,
                isFinalDispatch);

            var mqttClientService = _serviceProvider.GetRequiredService<IMqttClientService>();
            await mqttClientService.PublishOrderAsync(
                robot.ProtocolName,
                robot.ProtocolVersion,
                robot.RobotManufacturer,
                robot.RobotSerialNumber,
                order,
                ct: ct);

            await PersistRobotHeaderIdAsync(robot, nextHeaderId, ct);

            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);
                }
            }

            // 8. 设置冷却期
            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, orderId);

            return new IdleYieldResult
            {
                Success = true,
                OrderId = orderId,
                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 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 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;
            }

            _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))
            {
                await DeleteYieldCacheAsync(cacheKey);
                return false;
            }

            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 isFinalDispatch = IsFinalYieldResourceSegment(cache, junctionIndex, resourceIndex);
            var nextHeaderId = checked((int)(robot.HeaderId + 1));
            var order = BuildYieldOrder(
                robot,
                cache.YieldOrderId,
                nextHeaderId,
                cache.OrderUpdateId,
                segmentItem.Segments,
                graph,
                cache.MapCode,
                isFinalDispatch);

            var mqttClientService = _serviceProvider.GetRequiredService<IMqttClientService>();
            await mqttClientService.PublishOrderAsync(
                robot.ProtocolName,
                robot.ProtocolVersion,
                robot.RobotManufacturer,
                robot.RobotSerialNumber,
                order,
                ct: ct);

            await PersistRobotHeaderIdAsync(robot, nextHeaderId, ct);

            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))
            {
                await DeleteYieldCacheAsync(cacheKey);
            }

            _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<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,
            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 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 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;
    }

    /// <summary>
    /// 构建让行 VDA5050 Order。
    /// </summary>
    private Domain.Models.VDA5050.Order BuildYieldOrder(
        Robot robot,
        string orderId,
        int headerId,
        int orderUpdateId,
        List<PathSegmentWithCode> segments,
        PathGraph graph,
        string mapCode,
        bool isFinalDispatch)
    {
        var nodes = new List<Node>();
        var edges = new List<Edge>();
        var seqId = 0;
        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 = seqId++,
                    Released = true,
                    NodeDescription = "Yield start",
                    NodePosition = new NodePosition
                    {
                        X = fromNode.X / coordinateScale,
                        Y = fromNode.Y / coordinateScale,
                        Theta = 0,
                        MapId = mapCode,
                        AllowedDeviationXY = 0.5
                    },
                    Actions = new List<Domain.Models.VDA5050.Action>()
                });
            }

            // 添加边
            edges.Add(new Edge
            {
                EdgeId = seg.EdgeCode,
                SequenceId = seqId++,
                Released = true,
                StartNodeId = seg.FromNodeCode,
                EndNodeId = seg.ToNodeCode,
                MaxSpeed = seg.MaxSpeed,
                Orientation = 0
            });

            // 添加终点节点
            if (graph.Nodes.TryGetValue(seg.ToNodeId, out var toNode))
            {
                nodes.Add(new Node
                {
                    NodeId = seg.ToNodeCode,
                    SequenceId = seqId++,
                    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,
                        Theta = 0,
                        MapId = mapCode,
                        AllowedDeviationXY = 0.5
                    },
                    Actions = new List<Domain.Models.VDA5050.Action>()
                });
            }
        }

        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);
}