zengzy / hhrcs_service · Files

using System;
using System.Collections.Generic;
using System.Linq;
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 readonly IRobotCacheService _robotCacheService;
    private readonly IAgvPathService _agvPathService;
    private readonly IUnifiedTrafficControlService _trafficControl;
    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,
        IServiceProvider serviceProvider,
        IRobotRepository robotRepository,
        IConnectionMultiplexer redis,
        ILogger<IdleRobotYieldService> logger,
        IOptions<AppSettings> settings)
    {
        _robotCacheService = robotCacheService;
        _agvPathService = agvPathService;
        _trafficControl = trafficControl;
        _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. 构建并下发 VDA5050 Order
            var orderId = $"YIELD-{Guid.NewGuid():N}";
            var order = BuildYieldOrder(robot, orderId, pathSegments, graph, request.MapCode);

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

            // 7. 设置冷却期
            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 anchorNode = graph.Nodes.Values.FirstOrDefault(n =>
            string.Equals(n.NodeCode, blockingAnchorNodeCode, StringComparison.OrdinalIgnoreCase));
        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 distanceAnchor = anchorNode ?? idleCurrentNode;

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

        YieldTargetCandidateEvaluation? bestFallback = null;

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

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

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

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

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

            var geometricIntersection = SweptAreaCoverageResolver.ZonesIntersect(
                parkingSweptZones,
                remainingTaskSweptZones);

            _logger.LogDebug(
                "[让行] 评估让行候选: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Priority={Priority}, Distance={Distance}, ResourceIntersection={IntersectionCount}, GeometricIntersection={GeometricIntersection}",
                robot.RobotId,
                candidate.Node.NodeCode,
                candidate.Priority,
                candidate.DistanceSquared,
                intersectionCount,
                geometricIntersection);

            if (!geometricIntersection)
            {
                _logger.LogInformation(
                    "[让行] 选中零几何交集让行目标: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, Priority={Priority}, ResourceIntersection={IntersectionCount}",
                    robot.RobotId,
                    candidate.Node.NodeCode,
                    candidate.Priority,
                    intersectionCount);
                return candidate.Node.NodeCode;
            }

            var evaluated = new YieldTargetCandidateEvaluation(candidate, intersectionCount, geometricIntersection);
            if (bestFallback == null || CompareCandidate(evaluated, bestFallback) < 0)
            {
                bestFallback = evaluated;
            }
        }

        if (bestFallback != null)
        {
            _logger.LogInformation(
                "[让行] 未找到零几何交集目标，选中回退目标: IdleRobot={IdleRobotId}, Target={TargetNodeCode}, ResourceIntersection={IntersectionCount}, GeometricIntersection={GeometricIntersection}, Priority={Priority}",
                robot.RobotId,
                bestFallback.Candidate.Node.NodeCode,
                bestFallback.IntersectionCount,
                bestFallback.GeometricIntersection,
                bestFallback.Candidate.Priority);
            return bestFallback.Candidate.Node.NodeCode;
        }

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

    /// <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<bool> CanReachTargetAsync(
        Robot robot,
        Guid mapId,
        PathGraph graph,
        string fromNodeCode,
        double currentTheta,
        string toNodeCode,
        CancellationToken ct)
    {
        var segments = await PlanYieldPathAsync(
            robot,
            mapId,
            graph,
            fromNodeCode,
            currentTheta,
            toNodeCode,
            ct);
        return segments.Count > 0;
    }

    private static List<YieldTargetCandidate> BuildOrderedYieldCandidates(
        PathGraph graph,
        PathNode distanceAnchor,
        PathNode idleCurrentNode,
        string idleCurrentNodeCode,
        string blockingAnchorNodeCode)
    {
        var candidates = new List<YieldTargetCandidate>();
        var seenNodeCodes = new HashSet<string>(StringComparer.OrdinalIgnoreCase);

        void AddCandidate(PathNode node, int priority)
        {
            if (string.IsNullOrWhiteSpace(node.NodeCode) ||
                string.Equals(node.NodeCode, idleCurrentNodeCode, StringComparison.OrdinalIgnoreCase) ||
                string.Equals(node.NodeCode, blockingAnchorNodeCode, StringComparison.OrdinalIgnoreCase) ||
                !seenNodeCodes.Add(node.NodeCode))
            {
                return;
            }

            candidates.Add(new YieldTargetCandidate(
                node,
                priority,
                DistanceSquared(distanceAnchor, node)));
        }

        foreach (var node in graph.GetWaitingAreaNodes()
                     .Select(id => graph.Nodes.TryGetValue(id, out var n) ? n : null)
                     .Where(n => n != null)
                     .Select(n => n!)
                     .OrderBy(n => DistanceSquared(distanceAnchor, n))
                     .ThenBy(n => n.NodeCode, StringComparer.OrdinalIgnoreCase))
        {
            AddCandidate(node, 0);
        }

        foreach (var node in graph.Nodes.Values
                     .Where(n => !string.IsNullOrWhiteSpace(n.NodeCode))
                     .Where(n => n.NodeCode.Contains("parking", StringComparison.OrdinalIgnoreCase)
                              || n.NodeCode.Contains("wait", StringComparison.OrdinalIgnoreCase)
                              || n.NodeCode.Contains("yield", StringComparison.OrdinalIgnoreCase)
                              || n.NodeCode.Contains("side", StringComparison.OrdinalIgnoreCase)
                              || n.NodeCode.Contains("bay", StringComparison.OrdinalIgnoreCase))
                     .OrderBy(n => DistanceSquared(distanceAnchor, n))
                     .ThenBy(n => n.NodeCode, StringComparer.OrdinalIgnoreCase))
        {
            AddCandidate(node, 1);
        }

        foreach (var node in idleCurrentNode.OutEdges
                     .Select(edge => graph.Nodes.TryGetValue(edge.ToNodeId, out var neighbor) ? neighbor : null)
                     .Where(n => n != null)
                     .Select(n => n!)
                     .OrderBy(n => DistanceSquared(distanceAnchor, n))
                     .ThenBy(n => n.NodeCode, StringComparer.OrdinalIgnoreCase))
        {
            AddCandidate(node, 2);
        }

        foreach (var node in graph.Nodes.Values
                     .Where(n => n.Active && !string.IsNullOrWhiteSpace(n.NodeCode))
                     .OrderBy(n => DistanceSquared(distanceAnchor, n))
                     .ThenBy(n => n.NodeCode, StringComparer.OrdinalIgnoreCase))
        {
            AddCandidate(node, 3);
        }

        return candidates
            .OrderBy(candidate => candidate.Priority)
            .ThenBy(candidate => candidate.DistanceSquared)
            .ThenBy(candidate => candidate.Node.NodeCode, StringComparer.OrdinalIgnoreCase)
            .ToList();
    }

    private static int CountIntersection(HashSet<string> left, HashSet<string> right)
    {
        if (left.Count == 0 || right.Count == 0)
        {
            return 0;
        }

        var smaller = left.Count <= right.Count ? left : right;
        var larger = ReferenceEquals(smaller, left) ? right : left;
        var count = 0;
        foreach (var item in smaller)
        {
            if (larger.Contains(item))
            {
                count++;
            }
        }

        return count;
    }

    private static int CompareCandidate(YieldTargetCandidateEvaluation left, YieldTargetCandidateEvaluation right)
    {
        var byGeometry = left.GeometricIntersection.CompareTo(right.GeometricIntersection);
        if (byGeometry != 0)
        {
            return byGeometry;
        }

        var byIntersection = left.IntersectionCount.CompareTo(right.IntersectionCount);
        if (byIntersection != 0)
        {
            return byIntersection;
        }

        var byPriority = left.Candidate.Priority.CompareTo(right.Candidate.Priority);
        if (byPriority != 0)
        {
            return byPriority;
        }

        var byDistance = left.Candidate.DistanceSquared.CompareTo(right.Candidate.DistanceSquared);
        if (byDistance != 0)
        {
            return byDistance;
        }

        return StringComparer.OrdinalIgnoreCase.Compare(
            left.Candidate.Node.NodeCode,
            right.Candidate.Node.NodeCode);
    }

    /// <summary>
    /// 构建让行 VDA5050 Order。
    /// </summary>
    private Domain.Models.VDA5050.Order BuildYieldOrder(Robot robot, string orderId, List<PathSegmentWithCode> segments, PathGraph graph, string mapCode)
    {
        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 ? "Yield target" : "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 = (int)(robot.HeaderId + 1),
            Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffzzz"),
            Version = robot.ProtocolVersion,
            Manufacturer = robot.RobotManufacturer,
            SerialNumber = robot.RobotSerialNumber,
            ZoneSetId = mapCode,
            OrderId = orderId,
            OrderUpdateId = 0,
            Nodes = nodes,
            Edges = edges
        };
    }

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

    private sealed record YieldTargetCandidateEvaluation(
        YieldTargetCandidate Candidate,
        int IntersectionCount,
        bool GeometricIntersection);
}