zengzy / hhrcs_service · Files

using Microsoft.Extensions.Logging;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
using StackExchange.Redis;
using System.Text.Json;

namespace Rcs.Infrastructure.PathFinding.Services;

/// <summary>
/// 路口冲突解决器 - 实现智能避让决策
/// @author zzy
/// @date 2026-03-10
/// </summary>
public class JunctionConflictResolver
{
    private readonly IUnifiedTrafficControlService _trafficControl;
    private readonly IRobotCacheService _robotCache;
    private readonly IAgvPathService _pathService;
    private readonly IConnectionMultiplexer _redis;
    private readonly ILogger<JunctionConflictResolver> _logger;

    // Redis Key前缀
    private const string JunctionWaitKeyPrefix = "rcs:junction:wait:";
    private const string RobotPriorityKeyPrefix = "rcs:robot:priority:";

    // 配置参数
    private readonly JunctionConflictSettings _settings = new()
    {
        ShortWaitThresholdSeconds = 5,
        MediumWaitThresholdSeconds = 15,
        LongWaitThresholdSeconds = 30,
        DetectionRadiusMeters = 50.0,
        PreemptiveTriggerSeconds = 10,
        EnableLateralAvoidance = true,
        EnablePreemptiveAvoidance = true,
        EnableParkingAvoidance = true
    };

    public JunctionConflictResolver(
        IUnifiedTrafficControlService trafficControl,
        IRobotCacheService robotCache,
        IAgvPathService pathService,
        IConnectionMultiplexer redis,
        ILogger<JunctionConflictResolver> logger)
    {
        _trafficControl = trafficControl;
        _robotCache = robotCache;
        _pathService = pathService;
        _redis = redis;
        _logger = logger;
    }

    /// <summary>
    /// 解决路口冲突 - 核心决策方法
    /// @author zzy
    /// </summary>
    public async Task<ConflictResolutionResult> ResolveJunctionConflictAsync(
        Guid robotId,
        string mapCode,
        string junctionNodeCode,
        List<PathSegment> nextSegments,
        PathGraph graph)
    {
        try
        {
            // 1. 获取路口信息
            var junctionInfo = GetJunctionInfo(junctionNodeCode, graph);
            if (junctionInfo == null)
            {
                return ConflictResolutionResult.NoConflict();
            }

            // 2. 检测路口冲突
            var conflictInfo = await DetectJunctionConflictAsync(
                robotId, mapCode, junctionNodeCode, junctionInfo);

            if (!conflictInfo.HasConflict)
            {
                return ConflictResolutionResult.NoConflict();
            }

            // 3. 获取我的优先级信息
            var myPriority = await GetRobotPriorityInfoAsync(robotId);

            // 4. 比较优先级，判断是否需要避让
            var shouldYield = await ShouldYieldAsync(myPriority, conflictInfo.ConflictingRobots);

            if (!shouldYield)
            {
                // 优先级高，不需要避让，继续等待锁释放
                return ConflictResolutionResult.NoConflict();
            }

            // 5. 获取等待时长
            var waitDuration = await GetWaitDurationAsync(robotId, junctionNodeCode);

            // 6. 根据等待时长和路口类型选择避让策略
            var strategy = SelectAvoidanceStrategy(
                waitDuration, junctionInfo, conflictInfo);

            // 7. 根据策略查找避让位置
            var avoidanceNodeCode = await FindAvoidanceLocationAsync(
                robotId, mapCode, junctionNodeCode, strategy, junctionInfo, graph);

            return new ConflictResolutionResult
            {
                ShouldAvoid = true,
                Strategy = strategy,
                JunctionNodeCode = junctionNodeCode,
                AvoidanceNodeCode = avoidanceNodeCode,
                OriginalTargetNodeCode = nextSegments.FirstOrDefault()?.ToNodeId.ToString(),
                Reason = $"优先级较低，等待{waitDuration.TotalSeconds:F1}秒，采用{strategy}策略"
            };
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "路口冲突解决异常: RobotId={RobotId}, Junction={Junction}",
                robotId, junctionNodeCode);
            return ConflictResolutionResult.NoConflict();
        }
    }

    /// <summary>
    /// 获取路口信息
    /// @author zzy
    /// </summary>
    private JunctionInfo? GetJunctionInfo(string junctionNodeCode, PathGraph graph)
    {
        var node = graph.Nodes.Values.FirstOrDefault(n => n.NodeCode == junctionNodeCode);
        if (node == null || node.OutEdges.Count < 3)
        {
            return null;
        }

        var connectedNodes = node.OutEdges
            .Select(e => graph.Nodes.TryGetValue(e.ToNodeId, out var n) ? n.NodeCode : string.Empty)
            .Where(code => !string.IsNullOrEmpty(code))
            .ToList();

        // 识别可用于避让的岔路（非主干道节点）
        var avoidanceLanes = connectedNodes
            .Where(code => IsAvoidanceLane(code, graph))
            .ToList();

        // 识别临时停靠点（标记为停靠点的节点）
        var parkingSpots = connectedNodes
            .Where(code => IsParkingSpot(code, graph))
            .ToList();

        return new JunctionInfo
        {
            JunctionNodeCode = junctionNodeCode,
            Type = DetermineJunctionType(connectedNodes.Count),
            ConnectedNodeCodes = connectedNodes,
            AvoidanceLaneNodeCodes = avoidanceLanes,
            ParkingSpotNodeCodes = parkingSpots,
            JunctionRadius = 10.0
        };
    }

    /// <summary>
    /// 检测路口冲突
    /// @author zzy
    /// </summary>
    private async Task<JunctionConflictInfo> DetectJunctionConflictAsync(
        Guid robotId,
        string mapCode,
        string junctionNodeCode,
        JunctionInfo junctionInfo)
    {
        var conflictInfo = new JunctionConflictInfo
        {
            HasConflict = false,
            ConflictingRobots = new List<RobotPriorityInfo>()
        };

        // 检查路口节点是否被其他机器人占用
        var junctionOccupied = await _trafficControl.IsNodeOccupiedByOtherAsync(
            mapCode, junctionNodeCode, robotId);

        if (junctionOccupied)
        {
            conflictInfo.HasConflict = true;
            // 获取占用路口的机器人信息
            var occupyingRobots = await GetRobotsOccupyingJunctionAsync(
                mapCode, junctionNodeCode, robotId);
            conflictInfo.ConflictingRobots.AddRange(occupyingRobots);
        }

        // 检查连接路口的边是否存在逆向冲突
        foreach (var connectedNode in junctionInfo.ConnectedNodeCodes)
        {
            var edgeCode = GetEdgeCode(junctionNodeCode, connectedNode, null);
            if (string.IsNullOrEmpty(edgeCode)) continue;

            var hasReverseConflict = await _trafficControl.HasReverseConflictAsync(
                mapCode, edgeCode, junctionNodeCode, connectedNode, robotId);

            if (hasReverseConflict)
            {
                conflictInfo.HasConflict = true;
                conflictInfo.HasReverseConflict = true;
                break;
            }
        }

        return conflictInfo;
    }

    /// <summary>
    /// 获取机器人优先级信息
    /// @author zzy
    /// </summary>
    private async Task<RobotPriorityInfo> GetRobotPriorityInfoAsync(Guid robotId)
    {
        var db = _redis.GetDatabase();
        var key = $"{RobotPriorityKeyPrefix}{robotId}";
        var json = await db.StringGetAsync(key);

        if (!json.IsNullOrEmpty)
        {
            var cached = JsonSerializer.Deserialize<RobotPriorityInfo>(json.ToString());
            if (cached != null)
                return cached;
        }

        // 从缓存服务获取机器人信息
        var robotCaches = await _robotCache.GetAllActiveRobotCacheAsync();
        var robotCache = robotCaches.FirstOrDefault(item =>
            Guid.TryParse(item.Basic.RobotId, out var cacheRobotId) && cacheRobotId == robotId);
        var status = robotCache.Status;

        var priorityInfo = new RobotPriorityInfo
        {
            RobotId = robotId,
            Priority = 50, // 默认优先级
            IsLoaded = false,
            BatteryLevel = status?.BatteryLevel ?? 100,
            ArrivalTime = DateTime.Now
        };

        // 缓存5秒
        await db.StringSetAsync(key, JsonSerializer.Serialize(priorityInfo), TimeSpan.FromSeconds(5));

        return priorityInfo;
    }

    /// <summary>
    /// 判断是否应该让行
    /// @author zzy
    /// </summary>
    private async Task<bool> ShouldYieldAsync(
        RobotPriorityInfo myInfo,
        List<RobotPriorityInfo> conflictingRobots)
    {
        if (!conflictingRobots.Any())
            return false;

        var comparer = new RobotPriorityComparer();

        // 如果存在任何一个优先级比我高的机器人，我就需要让行
        foreach (var other in conflictingRobots)
        {
            if (comparer.Compare(myInfo, other) > 0)
            {
                return true; // 对方优先级更高，我需要让行
            }
        }

        return false;
    }

    /// <summary>
    /// 选择避让策略
    /// @author zzy
    /// </summary>
    private AvoidanceStrategy SelectAvoidanceStrategy(
        TimeSpan waitDuration,
        JunctionInfo junctionInfo,
        JunctionConflictInfo conflictInfo)
    {
        // 等待时间短（<5秒），原地等待
        if (waitDuration.TotalSeconds < _settings.ShortWaitThresholdSeconds)
        {
            return AvoidanceStrategy.Wait;
        }

        // 等待时间中等（5-15秒），尝试侧向避让
        if (waitDuration.TotalSeconds < _settings.MediumWaitThresholdSeconds)
        {
            if (_settings.EnableLateralAvoidance && junctionInfo.AvoidanceLaneNodeCodes.Any())
            {
                return AvoidanceStrategy.Lateral;
            }

            if (_settings.EnableParkingAvoidance && junctionInfo.ParkingSpotNodeCodes.Any())
            {
                return AvoidanceStrategy.Parking;
            }

            return AvoidanceStrategy.Wait;
        }

        // 等待时间长（15-30秒），尝试停靠或后退
        if (waitDuration.TotalSeconds < _settings.LongWaitThresholdSeconds)
        {
            if (_settings.EnableParkingAvoidance && junctionInfo.ParkingSpotNodeCodes.Any())
            {
                return AvoidanceStrategy.Parking;
            }

            return AvoidanceStrategy.Retreat;
        }

        // 等待超时（>30秒），重新规划
        return AvoidanceStrategy.Reroute;
    }

    /// <summary>
    /// 查找避让位置
    /// @author zzy
    /// </summary>
    private async Task<string?> FindAvoidanceLocationAsync(
        Guid robotId,
        string mapCode,
        string junctionNodeCode,
        AvoidanceStrategy strategy,
        JunctionInfo junctionInfo,
        PathGraph graph)
    {
        switch (strategy)
        {
            case AvoidanceStrategy.Lateral:
                return await FindAvailableLateralLaneAsync(
                    robotId, mapCode, junctionInfo.AvoidanceLaneNodeCodes);

            case AvoidanceStrategy.Parking:
                return await FindAvailableParkingSpotAsync(
                    robotId, mapCode, junctionInfo.ParkingSpotNodeCodes);

            case AvoidanceStrategy.Retreat:
                return await FindSafeRetreatNodeAsync(
                    robotId, mapCode, junctionNodeCode, graph);

            default:
                return null;
        }
    }

    /// <summary>
    /// 查找可用的侧向岔路
    /// @author zzy
    /// </summary>
    private async Task<string?> FindAvailableLateralLaneAsync(
        Guid robotId,
        string mapCode,
        List<string> laneNodeCodes)
    {
        foreach (var laneNode in laneNodeCodes)
        {
            var isOccupied = await _trafficControl.IsNodeOccupiedByOtherAsync(
                mapCode, laneNode, robotId);

            if (!isOccupied)
            {
                return laneNode;
            }
        }

        return null;
    }

    /// <summary>
    /// 查找可用的停靠点
    /// @author zzy
    /// </summary>
    private async Task<string?> FindAvailableParkingSpotAsync(
        Guid robotId,
        string mapCode,
        List<string> parkingSpotCodes)
    {
        foreach (var spotNode in parkingSpotCodes)
        {
            var isOccupied = await _trafficControl.IsNodeOccupiedByOtherAsync(
                mapCode, spotNode, robotId);

            if (!isOccupied)
            {
                return spotNode;
            }
        }

        return null;
    }

    /// <summary>
    /// 查找安全后退节点
    /// @author zzy
    /// </summary>
    private async Task<string?> FindSafeRetreatNodeAsync(
        Guid robotId,
        string mapCode,
        string junctionNodeCode,
        PathGraph graph)
    {
        var junctionNode = graph.Nodes.Values.FirstOrDefault(n => n.NodeCode == junctionNodeCode);
        if (junctionNode == null)
            return null;

        // 查找入边的起点节点（上一个节点）
        foreach (var inEdge in junctionNode.InEdges)
        {
            if (graph.Nodes.TryGetValue(inEdge.FromNodeId, out var fromNode))
            {
                // 检查该节点是否为安全节点（非路口）
                if (fromNode.OutEdges.Count < 3)
                {
                    var isOccupied = await _trafficControl.IsNodeOccupiedByOtherAsync(
                        mapCode, fromNode.NodeCode, robotId);

                    if (!isOccupied)
                    {
                        return fromNode.NodeCode;
                    }
                }
            }
        }

        return null;
    }

    #region 辅助方法

    /// <summary>
    /// 获取等待时长
    /// @author zzy
    /// </summary>
    private async Task<TimeSpan> GetWaitDurationAsync(Guid robotId, string junctionNodeCode)
    {
        var db = _redis.GetDatabase();
        var key = $"{JunctionWaitKeyPrefix}{junctionNodeCode}:{robotId}";
        var startTimeStr = await db.StringGetAsync(key);

        if (startTimeStr.IsNullOrEmpty)
        {
            // 首次等待，记录开始时间
            await db.StringSetAsync(key, DateTime.Now.Ticks.ToString(), TimeSpan.FromMinutes(2));
            return TimeSpan.Zero;
        }

        if (long.TryParse(startTimeStr.ToString(), out var ticks))
        {
            var startTime = new DateTime(ticks);
            return DateTime.Now - startTime;
        }

        return TimeSpan.Zero;
    }

    /// <summary>
    /// 获取占用路口的机器人列表
    /// @author zzy
    /// </summary>
    private async Task<List<RobotPriorityInfo>> GetRobotsOccupyingJunctionAsync(
        string mapCode,
        string junctionNodeCode,
        Guid excludeRobotId)
    {
        var robotIds = await _trafficControl.GetRobotsOccupyingNodeAsync(mapCode, junctionNodeCode);
        var filtered = robotIds
            .Where(id => id != excludeRobotId)
            .Distinct()
            .ToList();

        if (filtered.Count == 0)
        {
            return new List<RobotPriorityInfo>();
        }

        var result = new List<RobotPriorityInfo>();
        foreach (var robotId in filtered)
        {
            var info = await GetRobotPriorityInfoAsync(robotId);
            result.Add(info);
        }

        return result;
    }

    /// <summary>
    /// 判断节点是否为避让岔路
    /// @author zzy
    /// </summary>
    private bool IsAvoidanceLane(string nodeCode, PathGraph graph)
    {
        var node = graph.Nodes.Values.FirstOrDefault(n => n.NodeCode == nodeCode);
        if (node == null) return false;

        // 简化判断：出度<=2的节点可作为避让岔路
        return node.OutEdges.Count <= 2;
    }

    /// <summary>
    /// 判断节点是否为停靠点
    /// @author zzy
    /// </summary>
    private bool IsParkingSpot(string nodeCode, PathGraph graph)
    {
        // 简化实现：节点名称包含"parking"或"wait"的视为停靠点
        return nodeCode.Contains("parking", StringComparison.OrdinalIgnoreCase) ||
               nodeCode.Contains("wait", StringComparison.OrdinalIgnoreCase);
    }

    /// <summary>
    /// 确定路口类型
    /// @author zzy
    /// </summary>
    private JunctionType DetermineJunctionType(int connectedCount)
    {
        return connectedCount switch
        {
            2 => JunctionType.TwoWay,
            3 => JunctionType.ThreeWay,
            4 => JunctionType.FourWay,
            _ => JunctionType.MultiWay
        };
    }

    /// <summary>
    /// 获取边编码
    /// @author zzy
    /// </summary>
    private string GetEdgeCode(string fromNodeCode, string toNodeCode, PathGraph? graph)
    {
        // 简化实现：使用节点编码组合
        return $"{fromNodeCode}-{toNodeCode}";
    }

    #endregion
}

#region 数据模型

/// <summary>
/// 路口信息
/// @author zzy
/// </summary>
public class JunctionInfo
{
    public string JunctionNodeCode { get; set; } = string.Empty;
    public JunctionType Type { get; set; }
    public List<string> ConnectedNodeCodes { get; set; } = new();
    public List<string> AvoidanceLaneNodeCodes { get; set; } = new();
    public List<string> ParkingSpotNodeCodes { get; set; } = new();
    public double JunctionRadius { get; set; }
}

/// <summary>
/// 路口类型
/// @author zzy
/// </summary>
public enum JunctionType
{
    TwoWay = 2,
    ThreeWay = 3,
    FourWay = 4,
    MultiWay = 5
}

/// <summary>
/// 路口冲突信息
/// @author zzy
/// </summary>
public class JunctionConflictInfo
{
    public bool HasConflict { get; set; }
    public bool HasReverseConflict { get; set; }
    public List<RobotPriorityInfo> ConflictingRobots { get; set; } = new();
}

/// <summary>
/// 机器人优先级信息
/// @author zzy
/// </summary>
public class RobotPriorityInfo
{
    public Guid RobotId { get; set; }
    public int Priority { get; set; }
    public bool IsLoaded { get; set; }
    public int BatteryLevel { get; set; }
    public DateTime ArrivalTime { get; set; }
}

/// <summary>
/// 机器人优先级比较器
/// @author zzy
/// </summary>
public class RobotPriorityComparer
{
    /// <summary>
    /// 比较两个机器人的优先级
    /// 返回值：-1表示robot1优先级高，1表示robot2优先级高，0表示相同
    /// @author zzy
    /// </summary>
    public int Compare(RobotPriorityInfo robot1, RobotPriorityInfo robot2)
    {
        // 1. 任务优先级（数值越小优先级越高）
        if (robot1.Priority != robot2.Priority)
            return robot1.Priority.CompareTo(robot2.Priority);

        // 2. 载货状态（载货优先）
        if (robot1.IsLoaded != robot2.IsLoaded)
            return robot2.IsLoaded.CompareTo(robot1.IsLoaded);

        // 3. 电池电量（低电量优先）
        if (Math.Abs(robot1.BatteryLevel - robot2.BatteryLevel) > 5)
            return robot1.BatteryLevel.CompareTo(robot2.BatteryLevel);

        // 4. 到达时间（先到先行）
        if (robot1.ArrivalTime != robot2.ArrivalTime)
            return robot1.ArrivalTime.CompareTo(robot2.ArrivalTime);

        // 5. 机器人ID（兜底，保证确定性）
        return robot1.RobotId.CompareTo(robot2.RobotId);
    }
}

/// <summary>
/// 冲突解决结果
/// @author zzy
/// </summary>
public class ConflictResolutionResult
{
    public bool ShouldAvoid { get; set; }
    public AvoidanceStrategy Strategy { get; set; }
    public string JunctionNodeCode { get; set; } = string.Empty;
    public string? AvoidanceNodeCode { get; set; }
    public string? OriginalTargetNodeCode { get; set; }
    public string Reason { get; set; } = string.Empty;

    public static ConflictResolutionResult NoConflict() => new()
    {
        ShouldAvoid = false,
        Strategy = AvoidanceStrategy.None,
        Reason = "无冲突"
    };
}

/// <summary>
/// 路口冲突配置
/// @author zzy
/// </summary>
public class JunctionConflictSettings
{
    public int ShortWaitThresholdSeconds { get; set; } = 5;
    public int MediumWaitThresholdSeconds { get; set; } = 15;
    public int LongWaitThresholdSeconds { get; set; } = 30;
    public double DetectionRadiusMeters { get; set; } = 50.0;
    public int PreemptiveTriggerSeconds { get; set; } = 10;
    public bool EnableLateralAvoidance { get; set; } = true;
    public bool EnablePreemptiveAvoidance { get; set; } = true;
    public bool EnableParkingAvoidance { get; set; } = true;
}
public enum AvoidanceStrategy
{
    None = 0,
    Wait = 1,           // 原地等待
    Lateral = 2,        // 侧向避让
    Preemptive = 3,     // 提前占位
    Retreat = 4,        // 后退避让
    Parking = 5,        // 临时停靠
    Reroute = 6,        // 重新规划
    Negotiated = 7      // 协商通过
}
#endregion