zengzy / hhrcs_service · Files

using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using Microsoft.Extensions.Logging;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;

namespace Rcs.Infrastructure.PathFinding.Services
{
    

    /// <summary>
    /// AGV寻路服务实现（统一锁模式）
    /// @author zzy
    /// </summary>
    public class AgvPathService : IAgvPathService, IDisposable
    {
        private readonly ILogger<AgvPathService> _logger;
        private readonly IMapCacheService _mapCacheService;
        private readonly IUnifiedTrafficControlService _unifiedTrafficControlService;
        private readonly DeadlockDetector _deadlockDetector;
        private readonly AStarPathFinder _pathFinder;

        private readonly ConcurrentDictionary<Guid, PathGraph> _graphCache = new();
        private readonly ConcurrentDictionary<Guid, RobotRuntimeState> _robotStates = new();
        private readonly Timer _deadlockCheckTimer;

        /// <summary>
        /// 响应时间限制(毫秒)
        /// </summary>
        private const int ResponseTimeoutMs = 10000;

        /// <summary>
        /// 死锁检测间隔(毫秒)
        /// </summary>
        private const int DeadlockCheckIntervalMs = 500;

        public AgvPathService(
            ILogger<AgvPathService> logger,
            IMapCacheService mapCacheService,
            IUnifiedTrafficControlService unifiedTrafficControlService,
            ILoggerFactory loggerFactory)
        {
            _logger = logger;
            _mapCacheService = mapCacheService;
            _unifiedTrafficControlService = unifiedTrafficControlService;
            _deadlockDetector = new DeadlockDetector(loggerFactory.CreateLogger<DeadlockDetector>(), _unifiedTrafficControlService);
            _pathFinder = new AStarPathFinder(loggerFactory.CreateLogger<AStarPathFinder>());

            // 启动死锁检测定时器
            _deadlockCheckTimer = new Timer(OnDeadlockCheck, null, DeadlockCheckIntervalMs, DeadlockCheckIntervalMs);
        }

        /// <summary>
        /// 计算路径
        /// @author zzy
        /// </summary>
        public async Task<PathResult> CalculatePathAsync(PathRequest request, CancellationToken ct = default)
        {
            var sw = Stopwatch.StartNew();
            using var cts = CancellationTokenSource.CreateLinkedTokenSource(ct);
            cts.CancelAfter(ResponseTimeoutMs);

            try
            {
                var graph = await GetOrBuildGraphAsync(request.MapId);
                if (graph == null)
                {
                    return new PathResult { Success = false, ErrorMessage = "无法加载地图数据" };
                }

                // 构建全局路径上下文
                var globalContext = BuildGlobalPathContext(request.RobotId);

                var result = _pathFinder.FindPath(request, graph, globalContext);

                // 规划成功后,更新机器人的规划路径
                if (result.Success)
                {
                    UpdateRobotPlannedPath(request.RobotId, result.Segments);
                }
                else
                {
                    // 尝试寻找等待区域（使用统一锁服务）
                    var mapCode = graph.MapId.ToString(); // TODO: 应从请求或配置中获取实际MapCode
                    var waitNode = graph.FindNearestWaitingNode(request.StartNodeId, mapCode, _unifiedTrafficControlService, request.RobotId);
                    if (waitNode.HasValue)
                    {
                        var waitRequest = new PathRequest
                        {
                            RobotId = request.RobotId,
                            MapId = request.MapId,
                            StartNodeId = request.StartNodeId,
                            EndNodeId = waitNode.Value,
                            CurrentTheta = request.CurrentTheta,
                            IsLoaded = request.IsLoaded,
                            Priority = request.Priority,
                            BatteryLevel = request.BatteryLevel
                        };
                        var waitResult = _pathFinder.FindPath(waitRequest, graph, globalContext);
                        if (waitResult.Success)
                        {
                            waitResult.NeedWait = true;
                            waitResult.WaitNodeId = waitNode.Value;
                            waitResult.SuggestedWaitTime = request.MaxWaitTime;
                            UpdateRobotPlannedPath(request.RobotId, waitResult.Segments);
                            return waitResult;
                        }
                    }
                }

                result.ComputeTimeMs = sw.ElapsedMilliseconds;
                return result;
            }
            catch (OperationCanceledException)
            {
                return new PathResult
                {
                    Success = false,
                    ErrorMessage = "路径计算超时",
                    ComputeTimeMs = sw.ElapsedMilliseconds
                };
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "路径计算异常: RobotId={RobotId}", request.RobotId);
                return new PathResult
                {
                    Success = false,
                    ErrorMessage = $"路径计算异常: {ex.Message}",
                    ComputeTimeMs = sw.ElapsedMilliseconds
                };
            }
        }

        /// <summary>
        /// 重新规划路径
        /// @author zzy
        /// </summary>
        public async Task<PathResult> ReplanPathAsync(PathRequest request, string mapCode, string reason, CancellationToken ct = default)
        {
            _logger.LogInformation("触发路径重规划: RobotId={RobotId}, MapCode={MapCode}, Reason={Reason}", 
                request.RobotId, mapCode, reason);

            // 释放当前机器人的所有锁（统一锁）
            await _unifiedTrafficControlService.ReleaseAllRobotLocksAsync(request.RobotId);

            // 重新计算路径
            return await CalculatePathAsync(request, ct);
        }

        /// <summary>
        /// 锁定路径段（跨地图统一锁模式）
        /// 使用 MapCode:NodeCode/EdgeCode 作为Key进行跨厂家地图的统一锁管理
        /// @author zzy
        /// </summary>
        public async Task<LockRequestResult> LockPathSegmentsAsync(Guid robotId, string mapCode, List<PathSegmentWithCode> segments, int lockTimeMs = 10000)
        {

            var ttlSeconds = lockTimeMs / 1000;

            // 提取所有节点Code和边Code
            var nodeCodes = new HashSet<string>();
            var edgeCodes = new List<string>();

            foreach (var segment in segments)
            {
                if (!string.IsNullOrEmpty(segment.FromNodeCode))
                    nodeCodes.Add(segment.FromNodeCode);
                if (!string.IsNullOrEmpty(segment.ToNodeCode))
                    nodeCodes.Add(segment.ToNodeCode);
                if (!string.IsNullOrEmpty(segment.EdgeCode))
                    edgeCodes.Add(segment.EdgeCode);
            }

            // 使用统一交通管制服务进行锁定
            var result = await _unifiedTrafficControlService.TryAcquirePathLocksAsync(
                mapCode, nodeCodes, edgeCodes, robotId, ttlSeconds).ConfigureAwait(false);

            if (!result.Success)
            {
                _logger.LogWarning("统一锁定失败: RobotId={RobotId}, MapCode={MapCode}, Reason={Reason}, ConflictingRobots={ConflictingRobots}",
                    robotId, mapCode, result.FailureReason, string.Join(",", result.ConflictingRobotIds));
            }

            return result;
        }

        /// <summary>
        /// 释放路径段（跨地图统一锁模式）
        /// @author zzy
        /// </summary>
        public async Task<int> ReleasePathSegmentsAsync(Guid robotId, string mapCode, List<PathSegmentWithCode> segments)
        {
            var nodeCodes = new HashSet<string>();
            var edgeCodes = new List<string>();

            foreach (var segment in segments)
            {
                if (!string.IsNullOrEmpty(segment.FromNodeCode))
                    nodeCodes.Add(segment.FromNodeCode);
                if (!string.IsNullOrEmpty(segment.ToNodeCode))
                    nodeCodes.Add(segment.ToNodeCode);
                if (!string.IsNullOrEmpty(segment.EdgeCode))
                    edgeCodes.Add(segment.EdgeCode);
            }

            return await _unifiedTrafficControlService.ReleasePathLocksAsync(
                mapCode, nodeCodes, edgeCodes, robotId).ConfigureAwait(false);
        }

        /// <summary>
        /// 释放机器人的所有锁
        /// @author zzy
        /// </summary>
        public async Task<int> ReleaseAllLocksAsync(Guid robotId)
        {
            return await _unifiedTrafficControlService.ReleaseAllRobotLocksAsync(robotId).ConfigureAwait(false);
        }

        /// <summary>
        /// 检测路径冲突（跨地图）
        /// @author zzy
        /// </summary>
        public async Task<PathConflictResult> CheckPathConflictAsync(Guid robotId, string mapCode, List<PathSegmentWithCode> segments)
        {

            var nodeCodes = new HashSet<string>();
            var edgeCodes = new List<string>();

            foreach (var segment in segments)
            {
                if (!string.IsNullOrEmpty(segment.FromNodeCode))
                    nodeCodes.Add(segment.FromNodeCode);
                if (!string.IsNullOrEmpty(segment.ToNodeCode))
                    nodeCodes.Add(segment.ToNodeCode);
                if (!string.IsNullOrEmpty(segment.EdgeCode))
                    edgeCodes.Add(segment.EdgeCode);
            }

            return await _unifiedTrafficControlService.CheckPathConflictAsync(
                mapCode, nodeCodes, edgeCodes, robotId).ConfigureAwait(false);
        }

        /// <summary>
        /// 更新机器人状态
        /// @author zzy
        /// </summary>
        public void UpdateRobotState(RobotRuntimeState state)
        {
            _robotStates[state.RobotId] = state;
            _deadlockDetector.UpdateRobotState(state);
        }

        /// <summary>
        /// 检测并解决死锁
        /// @author zzy
        /// </summary>
        public Task<DeadlockResolution?> CheckAndResolveDeadlockAsync(Guid robotId)
        {
            var deadlocks = _deadlockDetector.DetectDeadlocks();
            var relevantDeadlock = deadlocks.FirstOrDefault(d => d.InvolvedRobots.Contains(robotId));

            if (relevantDeadlock == null) return Task.FromResult<DeadlockResolution?>(null);

            var resolution = _deadlockDetector.ResolveDeadlock(relevantDeadlock);
            if (resolution.Success && resolution.RobotToYield == robotId)
            {
                _logger.LogWarning("机器人 {RobotId} 需要退让, 原因: {Reason}", robotId, resolution.YieldReason);
            }

            return Task.FromResult<DeadlockResolution?>(resolution);
        }

        /// <summary>
        /// 获取或构建寻路图
        /// @author zzy
        /// </summary>
        public async Task<PathGraph?> GetOrBuildGraphAsync(Guid mapId)
        {
            if (_graphCache.TryGetValue(mapId, out var cached)) return cached;

            var mapData = await _mapCacheService.GetMapAsync(mapId);
            if (mapData == null) return null;

            var graph = BuildGraph(mapData);
            _graphCache[mapId] = graph;
            return graph;
        }

        /// <summary>
        /// 从缓存数据构建寻路图
        /// @author zzy
        /// </summary>
        private PathGraph BuildGraph(MapCacheData mapData)
        {
            var graph = new PathGraph { 
                MapId = mapData.MapId,
                MapCode = mapData.MapCode
            };

            // 构建节点
            foreach (var node in mapData.Nodes.Where(n => n.Active))
            {
                graph.Nodes[node.NodeId] = new PathNode
                {
                    NodeId = node.NodeId,
                    NodeCode = node.NodeCode,
                    X = node.X,
                    Y = node.Y,
                    Theta = node.Theta,
                    Active = node.Active,
                    IsReverseParking = node.IsReverseParking,
                    AllowRotate = node.AllowRotate,
                    MaxCoordinateOffset = node.MaxCoordinateOffset
                };
            }

            // 构建边
            foreach (var edge in mapData.Edges.Where(e => e.Active))
            {
                if (!graph.Nodes.TryGetValue(edge.FromNode, out var fromNode)) continue;
                if (!graph.Nodes.TryGetValue(edge.ToNode, out var toNode)) continue;

                // 数据库中存储的是度数，转换为弧度
                var orientationAnglesRad = edge.OrientationRads?.Select(a => a * Math.PI / 180.0).ToList()
                    ?? new List<double> { 0, Math.PI };
                var maxAngleDeviationRad = edge.MaxRadDeviation.HasValue
                    ? edge.MaxRadDeviation.Value * Math.PI / 180.0
                    : (double?)null;

                var pathEdge = new PathEdge
                {
                    EdgeId = edge.EdgeId,
                    EdgeCode = edge.EdgeCode,
                    FromNodeId = edge.FromNode,
                    ToNodeId = edge.ToNode,
                    Length = edge.Length,
                    Cost = edge.Cost ?? 1.0,
                    Active = edge.Active,
                    OrientationRads = orientationAnglesRad,
                    MaxSpeed = edge.MaxSpeed,
                    MaxRadDeviation = maxAngleDeviationRad,
                    StartTangentRad = CalculateStartTangentAngle(edge, fromNode, toNode),
                    DirectionRad = CalculateEndDirectionAngle(edge, fromNode, toNode)
                };

                graph.Edges[edge.EdgeId] = pathEdge;
                fromNode.OutEdges.Add(pathEdge);
                toNode.InEdges.Add(pathEdge);
            }

            // 构建资源区域（包含区域内节点集合）
            var activeNodes = graph.Nodes.Values.ToList();
            foreach (var resource in mapData.Resources)
            {
                var nodeIds = new List<Guid>();
                var polygon = resource.LocationCoordinates;
                if (polygon != null && polygon.Count >= 3)
                {
                    foreach (var node in activeNodes)
                    {
                        if (IsPointInPolygon(node.X, node.Y, polygon))
                        {
                            nodeIds.Add(node.NodeId);
                        }
                    }
                }
                graph.Resources.Add(new PathResource
                {
                    ResourceId = resource.ResourceId,
                    ResourceCode = resource.ResourceCode,
                    Type = resource.Type,
                    Capacity = resource.Capacity ?? 999,
                    MaxSpeed = resource.MaxSpeed,
                    CanRotate = resource.CanRotate,
                    NodeIds = nodeIds,
                    LocationCoordinates = resource.LocationCoordinates,
                    PreAction1Type = resource.PreAction1Type,
                    PreNetActions1 = resource.PreNetActions1,
                    PostAction1Type = resource.PostAction1Type,
                    PostNetActions1 = resource.PostNetActions1,
                    PreAction2Type = resource.PreAction2Type,
                    PreNetActions2 = resource.PreNetActions2,
                    PostAction2Type = resource.PostAction2Type,
                    PostNetActions2 = resource.PostNetActions2
                });
            }

            return graph;
        }

        /// <summary>
        /// 计算边的终点方向角（考虑曲线情况）
        /// @author zzy
        /// </summary>
        /// <param name="edge">边数据</param>
        /// <param name="fromNode">起点节点</param>
        /// <param name="toNode">终点节点</param>
        /// <returns>终点方向角(弧度)</returns>
        private static double CalculateEndDirectionAngle(MapEdgeCache edge, PathNode fromNode, PathNode toNode)
        {
            // 圆弧：控制点[0]=圆心, [1]=起点, [2]=终点
            if (edge.IsCurve && edge.ControlPoints != null && edge.ControlPoints.Count >= 3)
            {
                var center = edge.ControlPoints[0];
                var start = edge.ControlPoints[1];
                var end = edge.ControlPoints[2];

                // 计算圆心到终点的半径方向
                var radiusAngle = Math.Atan2(end.Y - center.Y, end.X - center.X);

                // 判断圆弧方向（通过叉积判断顺逆时针）
                var v1x = start.X - center.X;
                var v1y = start.Y - center.Y;
                var v2x = end.X - center.X;
                var v2y = end.Y - center.Y;
                var cross = v1x * v2y - v1y * v2x;

                // 切线垂直于半径：逆时针+90度，顺时针-90度
                return cross >= 0 ? radiusAngle + Math.PI / 2 : radiusAngle - Math.PI / 2;
            }

            // 直线：使用起点到终点方向
            return Math.Atan2(toNode.Y - fromNode.Y, toNode.X - fromNode.X);
        }

        /// <summary>
        /// 计算边的起点切线方向（考虑曲线情况）
        /// @author zzy
        /// </summary>
        private static double CalculateStartTangentAngle(MapEdgeCache edge, PathNode fromNode, PathNode toNode)
        {
            // 圆弧：控制点[0]=圆心, [1]=起点, [2]=终点
            if (edge.IsCurve && edge.ControlPoints != null && edge.ControlPoints.Count >= 3)
            {
                var center = edge.ControlPoints[0];
                var start = edge.ControlPoints[1];
                var end = edge.ControlPoints[2];

                // 计算圆心到起点的半径方向
                var radiusAngle = Math.Atan2(start.Y - center.Y, start.X - center.X);

                // 判断圆弧方向（通过叉积判断顺逆时针）
                var v1x = start.X - center.X;
                var v1y = start.Y - center.Y;
                var v2x = end.X - center.X;
                var v2y = end.Y - center.Y;
                var cross = v1x * v2y - v1y * v2x;

                // 切线垂直于半径：逆时针+90度，顺时针-90度
                return cross >= 0 ? radiusAngle + Math.PI / 2 : radiusAngle - Math.PI / 2;
            }

            // 直线：使用起点到终点方向
            return Math.Atan2(toNode.Y - fromNode.Y, toNode.X - fromNode.X);
        }

        private static bool IsPointInPolygon(double x, double y, List<PointCache> polygon)
        {
            var inside = false;
            var count = polygon.Count;

            for (int i = 0, j = count - 1; i < count; j = i++)
            {
                var xi = polygon[i].X;
                var yi = polygon[i].Y;
                var xj = polygon[j].X;
                var yj = polygon[j].Y;

                if (IsPointOnSegment(x, y, polygon[j], polygon[i]))
                {
                    return true;
                }

                var intersect = ((yi > y) != (yj > y)) &&
                                (x < (xj - xi) * (y - yi) / (yj - yi + double.Epsilon) + xi);
                if (intersect) inside = !inside;
            }

            return inside;
        }

        private static bool IsPointOnSegment(double x, double y, PointCache a, PointCache b)
        {
            const double epsilon = 1e-6;
            var cross = (x - a.X) * (b.Y - a.Y) - (y - a.Y) * (b.X - a.X);
            if (Math.Abs(cross) > epsilon) return false;

            var dot = (x - a.X) * (b.X - a.X) + (y - a.Y) * (b.Y - a.Y);
            if (dot < -epsilon) return false;

            var squaredLength = (b.X - a.X) * (b.X - a.X) + (b.Y - a.Y) * (b.Y - a.Y);
            return dot <= squaredLength + epsilon;
        }

        /// <summary>
        /// 死锁检测回调
        /// @author zzy
        /// </summary>
        private void OnDeadlockCheck(object? state)
        {
            try
            {
                var deadlocks = _deadlockDetector.DetectDeadlocks();
                foreach (var deadlock in deadlocks)
                {
                    _logger.LogWarning("检测到死锁: 涉及机器人 {Robots}",
                        string.Join(", ", deadlock.InvolvedRobots));
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "死锁检测异常");
            }
        }

        /// <summary>
        /// 构建全局路径上下文（支持跨地图冲突检测）
        /// @author zzy
        /// </summary>
        public GlobalPathContext BuildGlobalPathContext(Guid excludeRobotId)
        {
            var context = new GlobalPathContext();
            foreach (var (robotId, state) in _robotStates)
            {
                if (robotId == excludeRobotId) continue;

                var edges = new List<PlannedEdge>();

                // 优先使用带Code的路径信息（用于跨地图冲突检测）
                if (state.PlannedPathWithCode.Count >= 2)
                {
                    for (int i = 0; i < state.PlannedPathWithCode.Count - 1; i++)
                    {
                        var fromNode = state.PlannedPathWithCode[i];
                        var toNode = state.PlannedPathWithCode[i + 1];

                        // 计算该边的预估到达时间
                        var estimatedArrival = fromNode.EstimatedArrivalTime;
                        // 判断是否为当前正在行驶的边（第一条边且有行驶进度）
                        var isCurrentEdge = (i == 0 && state.CurrentEdgeTraveledDistance > 0);

                        edges.Add(new PlannedEdge
                        {
                            FromNodeId = fromNode.NodeId,
                            ToNodeId = toNode.NodeId,
                            FromNodeCode = fromNode.NodeCode,
                            ToNodeCode = toNode.NodeCode,
                            MapCode = state.MapCode,
                            TraveledDistance = isCurrentEdge ? state.CurrentEdgeTraveledDistance : 0,
                            CurrentSpeed = state.CurrentSpeed,
                            AverageSpeed = state.AverageSpeed,
                            IsBlocked = state.IsBlocked,
                            EstimatedArrivalTime = estimatedArrival
                        });
                    }
                }
                // 回退到传统路径（仅支持同地图冲突检测）
                else if (state.PlannedPath.Count >= 2)
                {
                    for (int i = 0; i < state.PlannedPath.Count - 1; i++)
                    {
                        var isCurrentEdge = (i == 0 && state.CurrentEdgeTraveledDistance > 0);
                        edges.Add(new PlannedEdge
                        {
                            FromNodeId = state.PlannedPath[i],
                            ToNodeId = state.PlannedPath[i + 1],
                            TraveledDistance = isCurrentEdge ? state.CurrentEdgeTraveledDistance : 0,
                            CurrentSpeed = state.CurrentSpeed,
                            AverageSpeed = state.AverageSpeed,
                            IsBlocked = state.IsBlocked
                        });
                    }
                }

                if (edges.Count > 0)
                {
                    context.RobotPlannedEdges[robotId] = edges;
                }

                // 记录机器人平均速度
                if (state.AverageSpeed > 0)
                {
                    context.RobotAverageSpeeds[robotId] = state.AverageSpeed;
                }
            }
            return context;
        }

        /// <summary>
        /// 更新机器人规划路径（基础版，仅支持同地图冲突检测）
        /// @author zzy
        /// </summary>
        private void UpdateRobotPlannedPath(Guid robotId, List<PathSegment> segments)
        {
            if (!_robotStates.TryGetValue(robotId, out var state))
            {
                state = new RobotRuntimeState { RobotId = robotId };
                _robotStates[robotId] = state;
            }

            state.PlannedPath.Clear();
            state.PlannedPathWithCode.Clear();
            if (segments.Count > 0)
            {
                state.PlannedPath.Add(segments[0].FromNodeId);
                foreach (var seg in segments)
                {
                    state.PlannedPath.Add(seg.ToNodeId);
                }
            }
        }

        /// <summary>
        /// 更新机器人规划路径（带Code版，支持跨地图冲突检测）
        /// @author zzy
        /// </summary>
        /// <param name="robotId">机器人ID</param>
        /// <param name="mapCode">地图编码</param>
        /// <param name="segments">路径段列表（带NodeCode）</param>
        /// <param name="graph">地图图结构（用于查找NodeCode）</param>
        public void UpdateRobotPlannedPathWithCode(Guid robotId, string mapCode, List<PathSegmentWithCode> segments, PathGraph? graph = null)
        {
            if (!_robotStates.TryGetValue(robotId, out var state))
            {
                state = new RobotRuntimeState { RobotId = robotId };
                _robotStates[robotId] = state;
            }

            state.MapCode = mapCode;
            state.PlannedPath.Clear();
            state.PlannedPathWithCode.Clear();

            if (segments.Count == 0) return;

            // 计算预估到达时间
            double accumulatedTime = 0;
            double averageSpeed = state.AverageSpeed > 0 ? state.AverageSpeed : 1.0;

            // 添加起始节点
            var firstSeg = segments[0];
            state.PlannedPath.Add(firstSeg.FromNodeId);
            state.PlannedPathWithCode.Add(new PlannedPathNode
            {
                NodeId = firstSeg.FromNodeId,
                NodeCode = firstSeg.FromNodeCode,
                EstimatedArrivalTime = 0
            });

            // 添加后续节点
            foreach (var seg in segments)
            {
                state.PlannedPath.Add(seg.ToNodeId);

                // 预估通过该边的时间
                var edgeTime = seg.Length / (seg.MaxSpeed ?? averageSpeed);
                accumulatedTime += edgeTime;

                state.PlannedPathWithCode.Add(new PlannedPathNode
                {
                    NodeId = seg.ToNodeId,
                    NodeCode = seg.ToNodeCode,
                    EstimatedArrivalTime = accumulatedTime
                });
            }
        }

        /// <summary>
        /// 使用地图图结构补全路径段的NodeCode信息
        /// @author zzy
        /// </summary>
        public List<PathSegmentWithCode> EnrichSegmentsWithCode(List<PathSegment> segments, PathGraph graph)
        {
            var result = new List<PathSegmentWithCode>();
            foreach (var seg in segments)
            {
                var fromNodeCode = graph.Nodes.TryGetValue(seg.FromNodeId, out var fromNode) ? fromNode.NodeCode : string.Empty;
                var toNodeCode = graph.Nodes.TryGetValue(seg.ToNodeId, out var toNode) ? toNode.NodeCode : string.Empty;
                var edgeCode = graph.Edges.TryGetValue(seg.EdgeId, out var edge) ? edge.EdgeCode : string.Empty;

                result.Add(PathSegmentWithCode.FromSegment(seg, edgeCode, fromNodeCode, toNodeCode));
            }
            return result;
        }

        /// <summary>
        /// 使指定地图的图缓存失效
        /// @author zzy
        /// </summary>
        public void InvalidateGraphCache(Guid mapId)
        {
            if (_graphCache.TryRemove(mapId, out _))
            {
                _logger.LogInformation("已清除地图 {MapId} 的图缓存", mapId);
            }
        }

        /// <summary>
        /// 清空所有图缓存
        /// @author zzy
        /// </summary>
        public void ClearAllGraphCache()
        {
            _graphCache.Clear();
            _logger.LogInformation("已清空所有图缓存");
        }

        public void Dispose()
        {
            _deadlockCheckTimer?.Dispose();
            _graphCache.Clear();
        }
    }
}