zengzy / hhrcs_service · Files

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

namespace Rcs.Infrastructure.PathFinding.Services;

/// <summary>
/// 导航服务 - 集成路口冲突避让策略
/// @author zzy
/// @date 2026-03-10
/// </summary>
public class NavigationService
{
    private readonly IAgvPathService _agvPathService;
    private readonly IUnifiedTrafficControlService _unifiedTrafficControl;
    private readonly IRobotCacheService _robotCacheService;
    private readonly ILogger<NavigationService> _logger;
    private readonly AStarPathFinder _pathFinder;
    private readonly IConnectionMultiplexer _redis;
    private readonly JunctionConflictResolver _conflictResolver;

    // Redis Key前缀
    private const string AvoidanceStateKeyPrefix = "rcs:avoidance:state:";
    private const string JunctionWaitKeyPrefix = "rcs:junction:wait:";

    public NavigationService(
        IAgvPathService agvPathService,
        IUnifiedTrafficControlService unifiedTrafficControl,
        IRobotCacheService robotCacheService,
        ILoggerFactory loggerFactory,
        ILogger<NavigationService> logger,
        IConnectionMultiplexer redis)
    {
        _agvPathService = agvPathService;
        _unifiedTrafficControl = unifiedTrafficControl;
        _robotCacheService = robotCacheService;
        _logger = logger;
        _redis = redis;
        _pathFinder = new AStarPathFinder(loggerFactory.CreateLogger<AStarPathFinder>());
        _conflictResolver = new JunctionConflictResolver(
            unifiedTrafficControl,
            robotCacheService,
            agvPathService,
            redis,
            loggerFactory.CreateLogger<JunctionConflictResolver>());
    }

    /// <summary>
    /// 核心导航循环：规划路径 + 冲突检测 + 避让决策
    /// @author zzy
    /// </summary>
    public async Task<NavigationResult> ExecuteNavigationCycleAsync(
        Guid robotId,
        MovementType movementType,
        Guid mapId,
        string mapCode,
        Guid currentNodeId,
        Guid targetNodeId,
        double currentTheta)
    {
        try
        {
            // 1. 检查是否处于避让状态
            var avoidanceState = await GetAvoidanceStateAsync(robotId);
            if (avoidanceState != null && !avoidanceState.IsCompleted)
            {
                return await HandleAvoidanceStateAsync(robotId, avoidanceState);
            }

            // 2. 构造寻路请求
            var request = new PathRequest
            {
                RobotId = robotId,
                MapId = mapId,
                StartNodeId = currentNodeId,
                EndNodeId = targetNodeId,
                CurrentTheta = currentTheta,
                MovementType = movementType,
                ForkRadOffsets = new List<double>()
            };

            // 3. 获取地图拓扑和全局上下文
            var graph = await _agvPathService.GetOrBuildGraphAsync(mapId);
            var globalContext = _agvPathService.BuildGlobalPathContext(robotId);

            // 4. A*路径规划（考虑冲突代价）
            var pathResult = _pathFinder.FindPath(request, graph, globalContext);

            if (!pathResult.Success || pathResult.Segments.Count == 0)
            {
                return NavigationResult.Wait("无可行路径");
            }

            // 5. 获取下一步要走的边
            var nextSegment = pathResult.Segments[0];
            var edgeCode = graph.Edges.TryGetValue(nextSegment.EdgeId, out var edge) ? edge.EdgeCode : string.Empty;
            var fromNodeCode = graph.Nodes.TryGetValue(nextSegment.FromNodeId, out var fromNode) ? fromNode.NodeCode : string.Empty;
            var toNodeCode = graph.Nodes.TryGetValue(nextSegment.ToNodeId, out var toNode) ? toNode.NodeCode : string.Empty;

            var isInPlaceSegment = nextSegment.FromNodeId == nextSegment.ToNodeId;
            if (isInPlaceSegment)
            {
                // 原地路径没有实际边，直接放行，避免误判为缺少EdgeCode
                toNodeCode = string.IsNullOrEmpty(toNodeCode) ? fromNodeCode : toNodeCode;

                if (string.IsNullOrEmpty(toNodeCode))
                {
                    _logger.LogWarning("原地路径节点缺少Code信息: FromNodeId={FromNodeId}, ToNodeId={ToNodeId}",
                        nextSegment.FromNodeId, nextSegment.ToNodeId);
                    return NavigationResult.Wait("原地路径节点缺少Code信息");
                }

                _logger.LogDebug("检测到原地路径，跳过路口冲突检测与资源锁定: RobotId={RobotId}, NodeCode={NodeCode}",
                    robotId, toNodeCode);
                return NavigationResult.Move(nextSegment, toNodeCode);
            }

            if (string.IsNullOrEmpty(edgeCode) || string.IsNullOrEmpty(toNodeCode))
            {
                _logger.LogWarning("边或节点缺少Code信息: EdgeId={EdgeId}, ToNodeId={ToNodeId}",
                    nextSegment.EdgeId, nextSegment.ToNodeId);
                return NavigationResult.Wait("边或节点缺少Code信息");
            }

            // 6. 判断目标节点是否为路口
            var isJunction = await IsJunctionNodeAsync(mapCode, toNodeCode, graph);

            if (isJunction)
            {
                // 7. 路口冲突检测与避让决策
                var conflictResult = await _conflictResolver.ResolveJunctionConflictAsync(
                    robotId, mapCode, toNodeCode, pathResult.Segments.Take(3).ToList(), graph);

                if (conflictResult.ShouldAvoid)
                {
                    _logger.LogInformation(
                        "路口冲突避让触发: RobotId={RobotId}, Junction={Junction}, Strategy={Strategy}, Reason={Reason}",
                        robotId, toNodeCode, conflictResult.Strategy, conflictResult.Reason);

                    // 执行避让策略
                    return await ExecuteAvoidanceStrategyAsync(
                        robotId, mapCode, conflictResult, graph);
                }
            }

            // 8. 无冲突或优先级高，尝试锁定资源
            var lockResult = await TryAcquirePathLocksAsync(
                robotId, mapCode, fromNodeCode, toNodeCode, edgeCode);

            if (lockResult.Success)
            {
                // 9. 锁定成功，返回移动指令
                return NavigationResult.Move(nextSegment, toNodeCode);
            }
            else
            {
                // 10. 锁定失败，等待或触发避让
                _logger.LogDebug("资源锁定失败: RobotId={RobotId}, Reason={Reason}",
                    robotId, lockResult.FailureReason);
                return NavigationResult.Wait($"资源竞争: {lockResult.FailureReason}");
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "导航循环执行异常: RobotId={RobotId}", robotId);
            return NavigationResult.Wait($"导航异常: {ex.Message}");
        }
    }

    /// <summary>
    /// 判断节点是否为路口（出度>=3）
    /// @author zzy
    /// </summary>
    private async Task<bool> IsJunctionNodeAsync(string mapCode, string nodeCode, PathGraph graph)
    {
        var node = graph.Nodes.Values.FirstOrDefault(n => n.NodeCode == nodeCode);
        if (node == null) return false;

        // 路口定义：出边数量>=3（三岔路口及以上）
        return node.OutEdges.Count >= 3;
    }

    /// <summary>
    /// 尝试锁定路径资源（节点+边）
    /// @author zzy
    /// </summary>
    private async Task<LockRequestResult> TryAcquirePathLocksAsync(
        Guid robotId,
        string mapCode,
        string fromNodeCode,
        string toNodeCode,
        string edgeCode)
    {
        var result = new LockRequestResult { Success = false };

        // 锁定目标节点
        var nodeLocked = await _unifiedTrafficControl.TryAcquireNodeLockAsync(
            mapCode, toNodeCode, robotId, ttlSeconds: 30);

        if (!nodeLocked)
        {
            result.FailureReason = $"节点 {toNodeCode} 被占用";
            result.ConflictingNodeCodes.Add(toNodeCode);
            return result;
        }

        // 锁定边（带方向信息）
        var edgeLocked = await _unifiedTrafficControl.TryAcquireEdgeLockWithDirectionAsync(
            mapCode, edgeCode, robotId, fromNodeCode, toNodeCode, ttlSeconds: 30);

        if (!edgeLocked)
        {
            // 边锁定失败，回滚节点锁
            await _unifiedTrafficControl.ReleaseNodeLockAsync(mapCode, toNodeCode, robotId);
            result.FailureReason = $"边 {edgeCode} 被占用";
            result.ConflictingEdgeCodes.Add(edgeCode);
            return result;
        }

        result.Success = true;
        result.LockedNodeCodes.Add(toNodeCode);
        result.LockedEdgeCodes.Add(edgeCode);
        return result;
    }

    /// <summary>
    /// 执行避让策略
    /// @author zzy
    /// </summary>
    private async Task<NavigationResult> ExecuteAvoidanceStrategyAsync(
        Guid robotId,
        string mapCode,
        ConflictResolutionResult conflictResult,
        PathGraph graph)
    {
        switch (conflictResult.Strategy)
        {
            case AvoidanceStrategy.Wait:
                return await ExecuteWaitStrategyAsync(robotId, conflictResult);

            case AvoidanceStrategy.Lateral:
                return await ExecuteLateralAvoidanceAsync(robotId, mapCode, conflictResult, graph);

            case AvoidanceStrategy.Preemptive:
                return await ExecutePreemptiveAvoidanceAsync(robotId, mapCode, conflictResult, graph);

            case AvoidanceStrategy.Parking:
                return await ExecuteParkingAvoidanceAsync(robotId, mapCode, conflictResult, graph);

            case AvoidanceStrategy.Retreat:
                return await ExecuteRetreatStrategyAsync(robotId, mapCode, conflictResult, graph);

            case AvoidanceStrategy.Reroute:
                return await ExecuteRerouteStrategyAsync(robotId, conflictResult);

            default:
                return NavigationResult.Wait("未知避让策略");
        }
    }

    #region 避让策略实现

    /// <summary>
    /// 原地等待策略
    /// @author zzy
    /// </summary>
    private async Task<NavigationResult> ExecuteWaitStrategyAsync(
        Guid robotId,
        ConflictResolutionResult conflictResult)
    {
        // 记录等待状态
        await SaveAvoidanceStateAsync(new AvoidanceState
        {
            RobotId = robotId,
            Strategy = AvoidanceStrategy.Wait,
            JunctionNodeCode = conflictResult.JunctionNodeCode,
            StartTime = DateTime.UtcNow,
            IsCompleted = false,
            Reason = conflictResult.Reason
        });

        return NavigationResult.Wait($"路口等待: {conflictResult.Reason}");
    }

    /// <summary>
    /// 侧向避让策略：占用空闲岔路
    /// @author zzy
    /// </summary>
    private async Task<NavigationResult> ExecuteLateralAvoidanceAsync(
        Guid robotId,
        string mapCode,
        ConflictResolutionResult conflictResult,
        PathGraph graph)
    {
        if (string.IsNullOrEmpty(conflictResult.AvoidanceNodeCode))
        {
            return NavigationResult.Wait("侧向避让失败：无可用岔路");
        }

        // 尝试锁定侧向岔路节点
        var locked = await _unifiedTrafficControl.TryAcquireNodeLockAsync(
            mapCode, conflictResult.AvoidanceNodeCode, robotId, ttlSeconds: 60);

        if (!locked)
        {
            return NavigationResult.Wait("侧向避让失败：岔路被占用");
        }

        // 记录避让状态
        await SaveAvoidanceStateAsync(new AvoidanceState
        {
            RobotId = robotId,
            Strategy = AvoidanceStrategy.Lateral,
            JunctionNodeCode = conflictResult.JunctionNodeCode,
            AvoidanceNodeCode = conflictResult.AvoidanceNodeCode,
            OriginalTargetNodeCode = conflictResult.OriginalTargetNodeCode,
            StartTime = DateTime.UtcNow,
            IsCompleted = false
        });

        // 构建到侧向岔路的路径段
        var avoidanceSegment = BuildAvoidanceSegment(
            robotId, conflictResult.AvoidanceNodeCode, graph);

        return NavigationResult.Avoidance(
            AvoidanceStrategy.Lateral,
            avoidanceSegment,
            $"侧向避让至 {conflictResult.AvoidanceNodeCode}");
    }

    /// <summary>
    /// 提前占位避让策略
    /// @author zzy
    /// </summary>
    private async Task<NavigationResult> ExecutePreemptiveAvoidanceAsync(
        Guid robotId,
        string mapCode,
        ConflictResolutionResult conflictResult,
        PathGraph graph)
    {
        // 与侧向避让类似，但优先级判断更严格
        return await ExecuteLateralAvoidanceAsync(robotId, mapCode, conflictResult, graph);
    }

    /// <summary>
    /// 临时停靠避让策略
    /// @author zzy
    /// </summary>
    private async Task<NavigationResult> ExecuteParkingAvoidanceAsync(
        Guid robotId,
        string mapCode,
        ConflictResolutionResult conflictResult,
        PathGraph graph)
    {
        if (string.IsNullOrEmpty(conflictResult.AvoidanceNodeCode))
        {
            return NavigationResult.Wait("停靠避让失败：无可用停靠点");
        }

        // 锁定停靠点（较长TTL）
        var locked = await _unifiedTrafficControl.TryAcquireNodeLockAsync(
            mapCode, conflictResult.AvoidanceNodeCode, robotId, ttlSeconds: 120);

        if (!locked)
        {
            return NavigationResult.Wait("停靠避让失败：停靠点被占用");
        }

        await SaveAvoidanceStateAsync(new AvoidanceState
        {
            RobotId = robotId,
            Strategy = AvoidanceStrategy.Parking,
            JunctionNodeCode = conflictResult.JunctionNodeCode,
            AvoidanceNodeCode = conflictResult.AvoidanceNodeCode,
            StartTime = DateTime.UtcNow,
            IsCompleted = false
        });

        var avoidanceSegment = BuildAvoidanceSegment(
            robotId, conflictResult.AvoidanceNodeCode, graph);

        return NavigationResult.Avoidance(
            AvoidanceStrategy.Parking,
            avoidanceSegment,
            $"停靠避让至 {conflictResult.AvoidanceNodeCode}");
    }

    /// <summary>
    /// 后退避让策略
    /// @author zzy
    /// </summary>
    private async Task<NavigationResult> ExecuteRetreatStrategyAsync(
        Guid robotId,
        string mapCode,
        ConflictResolutionResult conflictResult,
        PathGraph graph)
    {
        if (string.IsNullOrEmpty(conflictResult.AvoidanceNodeCode))
        {
            return NavigationResult.Wait("后退避让失败：无安全节点");
        }

        await SaveAvoidanceStateAsync(new AvoidanceState
        {
            RobotId = robotId,
            Strategy = AvoidanceStrategy.Retreat,
            JunctionNodeCode = conflictResult.JunctionNodeCode,
            AvoidanceNodeCode = conflictResult.AvoidanceNodeCode,
            StartTime = DateTime.UtcNow,
            IsCompleted = false
        });

        // 构建后退路径（倒车）
        var retreatSegment = BuildRetreatSegment(
            robotId, conflictResult.AvoidanceNodeCode, graph);

        return NavigationResult.Avoidance(
            AvoidanceStrategy.Retreat,
            retreatSegment,
            $"后退避让至 {conflictResult.AvoidanceNodeCode}");
    }

    /// <summary>
    /// 重新规划策略
    /// @author zzy
    /// </summary>
    private async Task<NavigationResult> ExecuteRerouteStrategyAsync(
        Guid robotId,
        ConflictResolutionResult conflictResult)
    {
        await SaveAvoidanceStateAsync(new AvoidanceState
        {
            RobotId = robotId,
            Strategy = AvoidanceStrategy.Reroute,
            JunctionNodeCode = conflictResult.JunctionNodeCode,
            StartTime = DateTime.UtcNow,
            IsCompleted = false
        });

        return NavigationResult.Reroute($"重新规划绕行，避开路口 {conflictResult.JunctionNodeCode}");
    }

    #endregion

    #region 辅助方法

    /// <summary>
    /// 处理避让状态中的机器人
    /// @author zzy
    /// </summary>
    private async Task<NavigationResult> HandleAvoidanceStateAsync(
        Guid robotId,
        AvoidanceState avoidanceState)
    {
        var elapsed = DateTime.UtcNow - avoidanceState.StartTime;

        // 检查避让是否超时
        if (elapsed.TotalSeconds > 60)
        {
            _logger.LogWarning("避让超时，清除避让状态: RobotId={RobotId}, Strategy={Strategy}",
                robotId, avoidanceState.Strategy);
            await ClearAvoidanceStateAsync(robotId);
            return NavigationResult.Wait("避让超时，重新规划");
        }

        // 根据避让策略返回相应结果
        return avoidanceState.Strategy switch
        {
            AvoidanceStrategy.Wait => NavigationResult.Wait("等待中..."),
            AvoidanceStrategy.Lateral => NavigationResult.Wait("侧向避让中..."),
            AvoidanceStrategy.Parking => NavigationResult.Wait("停靠避让中..."),
            _ => NavigationResult.Wait("避让中...")
        };
    }

    /// <summary>
    /// 构建避让路径段
    /// @author zzy
    /// </summary>
    private PathSegment BuildAvoidanceSegment(
        Guid robotId,
        string targetNodeCode,
        PathGraph graph)
    {
        var targetNode = graph.Nodes.Values.FirstOrDefault(n => n.NodeCode == targetNodeCode);
        if (targetNode == null)
        {
            throw new InvalidOperationException($"目标节点不存在: {targetNodeCode}");
        }

        // 简化实现：返回一个指向目标节点的路径段
        return new PathSegment
        {
            ToNodeId = targetNode.NodeId,
            Length = 0,
            Angle = 0
        };
    }

    /// <summary>
    /// 构建后退路径段
    /// @author zzy
    /// </summary>
    private PathSegment BuildRetreatSegment(
        Guid robotId,
        string safeNodeCode,
        PathGraph graph)
    {
        var safeNode = graph.Nodes.Values.FirstOrDefault(n => n.NodeCode == safeNodeCode);
        if (safeNode == null)
        {
            throw new InvalidOperationException($"安全节点不存在: {safeNodeCode}");
        }

        // 后退路径：Angle = π（倒车）
        return new PathSegment
        {
            ToNodeId = safeNode.NodeId,
            Length = 0,
            Angle = Math.PI
        };
    }

    /// <summary>
    /// 保存避让状态到Redis
    /// @author zzy
    /// </summary>
    private async Task SaveAvoidanceStateAsync(AvoidanceState state)
    {
        var db = _redis.GetDatabase();
        var key = $"{AvoidanceStateKeyPrefix}{state.RobotId}";
        var json = JsonSerializer.Serialize(state);
        await db.StringSetAsync(key, json, TimeSpan.FromMinutes(5));
    }

    /// <summary>
    /// 获取避让状态
    /// @author zzy
    /// </summary>
    private async Task<AvoidanceState?> GetAvoidanceStateAsync(Guid robotId)
    {
        var db = _redis.GetDatabase();
        var key = $"{AvoidanceStateKeyPrefix}{robotId}";
        var json = await db.StringGetAsync(key);

        if (json.IsNullOrEmpty)
            return null;

        return JsonSerializer.Deserialize<AvoidanceState>(json.ToString());
    }

    /// <summary>
    /// 清除避让状态
    /// @author zzy
    /// </summary>
    private async Task ClearAvoidanceStateAsync(Guid robotId)
    {
        var db = _redis.GetDatabase();
        var key = $"{AvoidanceStateKeyPrefix}{robotId}";
        await db.KeyDeleteAsync(key);
    }

    #endregion
}

#region 数据模型

/// <summary>
/// 导航结果
/// @author zzy
/// </summary>
public class NavigationResult
{
    public NavigationAction Action { get; set; }
    public PathSegment? NextSegment { get; set; }
    public string? TargetNodeCode { get; set; }
    public AvoidanceStrategy? AvoidanceStrategy { get; set; }
    public string Message { get; set; } = string.Empty;

    public static NavigationResult Move(PathSegment segment, string targetNodeCode) => new()
    {
        Action = NavigationAction.Move,
        NextSegment = segment,
        TargetNodeCode = targetNodeCode,
        Message = "正常移动"
    };

    public static NavigationResult Wait(string message) => new()
    {
        Action = NavigationAction.Wait,
        Message = message
    };

    public static NavigationResult Avoidance(
        AvoidanceStrategy strategy,
        PathSegment? segment,
        string message) => new()
    {
        Action = NavigationAction.Avoidance,
        AvoidanceStrategy = strategy,
        NextSegment = segment,
        Message = message
    };

    public static NavigationResult Reroute(string message) => new()
    {
        Action = NavigationAction.Reroute,
        Message = message
    };
}

/// <summary>
/// 导航动作类型
/// @author zzy
/// </summary>
public enum NavigationAction
{
    Move = 1,       // 正常移动
    Wait = 2,       // 等待
    Avoidance = 3,  // 避让
    Reroute = 4     // 重新规划
}

/// <summary>
/// 避让策略枚举
/// @author zzy
/// </summary>
public enum AvoidanceStrategy
{
    None = 0,
    Wait = 1,           // 原地等待
    Lateral = 2,        // 侧向避让
    Preemptive = 3,     // 提前占位
    Retreat = 4,        // 后退避让
    Parking = 5,        // 临时停靠
    Reroute = 6,        // 重新规划
    Negotiated = 7      // 协商通过
}

/// <summary>
/// 避让状态记录
/// @author zzy
/// </summary>
public class AvoidanceState
{
    public Guid RobotId { 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 DateTime StartTime { get; set; }
    public DateTime? EndTime { get; set; }
    public bool IsCompleted { get; set; }
    public string? Reason { get; set; }
}

#endregion