zengzy / hhrcs_service · Files

using System.Collections.Concurrent;
using System.Text.Json;
using Microsoft.Extensions.DependencyInjection;
using Microsoft.Extensions.Logging;
using Microsoft.Extensions.Options;
using MQTTnet.Client;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
using Rcs.Application.Services.PathFind.Realtime;
using Rcs.Application.Services.Protocol;
using Rcs.Application.Shared;
using Rcs.Domain.Entities;
using Rcs.Domain.Models.VDA5050;
using Rcs.Domain.Repositories;
using Rcs.Domain.Settings;
using Rcs.Infrastructure.Mqtt.ParseFactory;
using Rcs.Infrastructure.Mqtt.ProcessorFactory;
using Rcs.Infrastructure.PathFinding.Services;
using Rcs.Infrastructure.Services.Protocol;
using StackExchange.Redis;
using Task = System.Threading.Tasks.Task;
using TaskStatus = System.Threading.Tasks.TaskStatus;

namespace Rcs.Infrastructure.Mqtt
{
    /// <summary>
    /// MQTT消息处理器，支持动态注册和动态字符串
    /// </summary>
    public class MqttMessageHandler : IMqttMessageHandler
    {
        private readonly ILogger<MqttMessageHandler> _logger;
        private readonly IServiceScopeFactory _scopeFactory;
        private readonly IStateParserFactory _stateParserFactory;
        private readonly IVisualizationParserFactory _visualizationParserFactory;
        private readonly IRobotCacheService _robotCacheService;
        private readonly IAgvPathService _agvPathService;
        private readonly IVisualizationProcessorFactory _visualizationProcessorFactory;
        private readonly ConcurrentDictionary<string, Func<string, string, string, Task<MqttHandleResult>>> _handlers;
        private readonly ConcurrentDictionary<string, int> _topicHeaderId = new()
        {
            ["connection"] = 0,
            ["state"] = 0,
            ["factsheet"] = 0,
            ["visualization"] = 0
        };
        private DateTime _lastLogTime = DateTime.MinValue;
        private readonly IConnectionMultiplexer _redis;
        private readonly AppSettings _settings;

        /// <summary>
        /// 全局导航协调器（用于上报状态与冲突事件）。
        /// </summary>
        private readonly IGlobalNavigationCoordinator _globalNavigationCoordinator;
        private readonly TimeSpan _logInterval = TimeSpan.FromSeconds(10);

        public MqttMessageHandler(
            ILogger<MqttMessageHandler> logger,
            IServiceScopeFactory scopeFactory,
            IStateParserFactory stateParserFactory,
            IVisualizationParserFactory visualizationParserFactory,
            IRobotCacheService robotCacheService,
            IAgvPathService agvPathService,
            IVisualizationProcessorFactory visualizationProcessorFactory,
            IGlobalNavigationCoordinator globalNavigationCoordinator,
            IConnectionMultiplexer redis,
            IOptions<AppSettings> settings)
        {
            _logger = logger;
            _scopeFactory = scopeFactory;
            _stateParserFactory = stateParserFactory;
            _visualizationParserFactory = visualizationParserFactory;
            _robotCacheService = robotCacheService;
            _agvPathService = agvPathService;
            _visualizationProcessorFactory = visualizationProcessorFactory;
            _globalNavigationCoordinator = globalNavigationCoordinator;
            _handlers = new ConcurrentDictionary<string, Func<string, string, string, Task<MqttHandleResult>>>();
            _redis = redis;
            _settings = settings.Value;
            // 注册默认处理器
            RegisterDefaultHandlers();
        }

        /// <summary>
        /// 处理接收到的MQTT消息
        /// </summary>
        public async Task HandleMessageAsync(MqttApplicationMessageReceivedEventArgs e)
        {
            try
            {
                var topic = e.ApplicationMessage.Topic;
                var payload = ExtractPayload(e.ApplicationMessage);
                if (!topic.Contains("visualization"))
                {
                    var now = DateTime.Now;
                     if (now - _lastLogTime >= _logInterval)
                     {
                         _logger.LogInformation("[MQTT] Recive | Topic={Topic} | Payload={Payload}", topic, payload);
                         _lastLogTime = now;
                    }
                }

                var result = await ProcessMessageAsync(topic, payload);

                if (!result.Success)
                {
                    _logger.LogWarning("[Advanced MQTT Handler] 消息处理失败 | Topic={Topic} | Error={Error}",
                        topic, result.Message);
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "[Advanced MQTT Handler] 消息处理过程中发生严重错误");
            }
        }

        /// <summary>
        /// 注册消息类型处理器
        /// </summary>
        /// <param name="messageType">消息类型</param>
        /// <param name="handler">处理器函数</param>
        public void RegisterHandler(string messageType, Func<string, string, string, Task<MqttHandleResult>> handler)
        {
            _handlers.AddOrUpdate(messageType.ToLowerInvariant(), handler, (key, oldValue) => handler);
            _logger.LogInformation("[Advanced MQTT Handler] 注册处理器 | MessageType={MessageType}", messageType);
        }

        /// <summary>
        /// 处理消息
        /// </summary>
        private async Task<MqttHandleResult> ProcessMessageAsync(string topic, string payload)
        {
            var topicParts = topic.Split('/');

            if (topicParts.Length < 5)
            {
                return MqttHandleResult.CreateFailure($"主题格式不正确: {topic}");
            }

            var robotSerialNumber = topicParts[3];
            var messageType = topicParts[4].ToLowerInvariant();
            // robot制造商
            var robotManufacturer = topicParts[2];

            if (_handlers.TryGetValue(messageType, out var handler))
            {
                return await handler(robotManufacturer,robotSerialNumber, payload);
            }
            else
            {
                return MqttHandleResult.CreateFailure($"未找到消息类型 '{messageType}' 的处理器");
            }
        }

        /// <summary>
        /// 注册默认处理器
        /// </summary>
        private void RegisterDefaultHandlers()
        {
            RegisterHandler("connection", HandleConnectionMessageAsync);
            RegisterHandler("state", HandleStateMessageAsync);
            RegisterHandler("factsheet", HandleFactsheetMessageAsync);
            RegisterHandler("visualization", HandleVisualizationMessageAsync);
        }

        /// <summary>
        /// 处理连接状态消息
        /// </summary>
        private async Task<MqttHandleResult> HandleConnectionMessageAsync(string robotManufacturer, string robotSerialNumber, string payload)
        {
            try
            {
                var connectionInfo = JsonSerializer.Deserialize<Connection>(payload);
                if (connectionInfo == null) throw new Exception();
                // 重置该机器人所有topic的headerId为0
                if (!string.IsNullOrEmpty(robotManufacturer) && !string.IsNullOrEmpty(robotSerialNumber))
                {
                    await ResetHeaderIdsAsync(robotManufacturer,robotSerialNumber);
                }
                var robotStatus = connectionInfo.ConnectionState switch
                {
                    "ONLINE" => OnlineStatus.Online,
                    "OFFLINE" => OnlineStatus.Offline,
                    "CONNECTIONBROKEN" => OnlineStatus.Connectionbroken,
                    _ => OnlineStatus.Offline
                };

                return MqttHandleResult.CreateSuccess($"更新机器人连接状态: {robotSerialNumber} -> {connectionInfo.ConnectionState}");

            }
            catch (Exception ex)
            {
                return MqttHandleResult.CreateFailure($"处理MQTT连接消息失败: {ex.Message}", ex);
            }
        }

        /// <summary>
        /// 处理状态消息（直接使用制造商+序列号更新缓存）
        /// @author zzy
        /// </summary>
        private async Task<MqttHandleResult> HandleStateMessageAsync(string robotManufacturer, string robotSerialNumber, string payload)
        {
            try
            {
                
                // 使用工厂解析特定制造商的状态数据
                var stateInfo = _stateParserFactory.ParseState(robotManufacturer, payload);
                if (stateInfo == null) throw new Exception("状态数据解析失败");
                // 可选：进行headerId去重判断
                if (!await IsNewHeaderIdAsync("state", robotManufacturer, robotSerialNumber, stateInfo.HeaderId))
                    throw new Exception($"状态消息HeaderId: {stateInfo.HeaderId} 消息过期");
                // 解析状态
                var robotStatus = DetermineRobotStatus(stateInfo);
                var operatingMode = ParseOperatingMode(stateInfo.OperatingMode);
                var errors = stateInfo.Errors.Any() ? JsonSerializer.Serialize(stateInfo.Errors) : null;
                var previousStatus = await _robotCacheService.GetStatusAsync(robotManufacturer, robotSerialNumber);
                var previousLastNode = previousStatus?.LastNode;

                // 直接使用制造商+序列号更新状态到Redis缓存
                await _robotCacheService.UpdateStatusAsync(
                    robotManufacturer,
                    robotSerialNumber,
                    robotStatus,
                    OnlineStatus.Online,
                    (int?)stateInfo.BatteryState?.BatteryCharge,
                    stateInfo.Driving,
                    stateInfo.Paused,
                    stateInfo.BatteryState?.Charging,
                    operatingMode,
                    errors,
                    stateInfo.LastNodeId);
                
                // 状态上报时尝试续期路径锁（无论是否行驶，拥堵等待时也可能仍占用路径）
                // RenewAllRobotLocksAsync 仅会续期该机器人当前持有的锁，无锁时不会产生副作用
                // @author zzy
                // @date 2026-03-05
                var basicInfo = await _robotCacheService.GetBasicAsync(robotManufacturer, robotSerialNumber);
                if (basicInfo != null && Guid.TryParse(basicInfo.RobotId, out var robotId))
                {
                    await _globalNavigationCoordinator.PublishStateEventAsync(new RobotStateNavigationEvent
                    {
                        RobotId = robotId,
                        RobotManufacturer = robotManufacturer,
                        RobotSerialNumber = robotSerialNumber,
                        LastNodeCode = stateInfo.LastNodeId,
                        IsDriving = stateInfo.Driving,
                        OccurredAtUtc = DateTime.Now
                    });

                    if (!string.Equals(previousLastNode, stateInfo.LastNodeId, StringComparison.OrdinalIgnoreCase))
                    {
                        await ReleaseExecutedLocksBeforeLastNodeAsync(
                            robotId,
                            robotManufacturer,
                            robotSerialNumber,
                            stateInfo.LastNodeId);
                    }

                    await _agvPathService.RenewAllLocksAsync(robotId, 30);
                }

                // 处理NewBaseRequest：当车辆快到当前段终点时，判断并发送下一段路径指令
                //if ((bool)stateInfo.NewBaseRequest)
                //{
                    await HandleNewBaseRequestAsync(robotManufacturer, robotSerialNumber);
                //}

                var hasInProgressTask = await HasInProgressTaskWithInProgressSubTaskAsync(
                    robotManufacturer,
                    robotSerialNumber);

                // await TryCreateAutoNodeCalibrationOrderAsync(
                //     robotManufacturer,
                //     robotSerialNumber,
                //     stateInfo,
                //     robotStatus,
                //     hasInProgressTask);

                // VDA5050 Order 完成自动判断
                // 条件预筛选：仅在机器人空闲（非行驶、非错误）时进行完成判断
                // 注意：原地路径场景下 State.OrderId 可能不是当前子任务，不能作为硬条件
                // @author zzy
                if (!stateInfo.Driving && robotStatus != RobotStatus.Error && hasInProgressTask)
                {
                    await TryHandleOrderCompletionAsync(robotManufacturer, robotSerialNumber, stateInfo);
                }

                return MqttHandleResult.CreateSuccess($"成功更新机器人状态: {robotManufacturer}:{robotSerialNumber}");
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "处理制造商 '{Manufacturer}' 的状态消息失败", robotManufacturer);
                return MqttHandleResult.CreateFailure($"处理状态消息失败: {ex.Message}", ex);
            }
        }

        /// <summary>
        /// 处理设备信息消息
        /// </summary>
        private Task<MqttHandleResult> HandleFactsheetMessageAsync(string robotManufacturer, string robotSerialNumber, string payload)
        {
            try
            {
                //var factSheet = JsonSerializer.Deserialize<FactSheet>(payload);
                //if (factSheet == null) throw new SharedException(10001);
                //if (!IsNewHeaderId("factsheet", factSheet.HeaderId)) throw new SharedException(10002);

                //using var scope = _serviceProvider.CreateScope();
                //var robotRepository = scope.ServiceProvider.GetRequiredService<IRobotRepository>();

                //var robot = await robotRepository.GetBySerialNumberAsync(robotSerialNumber);
                //if (robot == null)
                //{
                //    throw new ExtendRobotException(robotSerialNumber, 20003);
                //}
                //robot.UpdateFactSheet(factSheet);
                //await robotRepository.UpdateAsync(robot);

                return Task.FromResult(MqttHandleResult.CreateSuccess());
            }
            catch (Exception ex)
            {
                return Task.FromResult(MqttHandleResult.CreateFailure($"处理设备信息失败: {ex.Message}", ex));
            }
        }

        /// <summary>
        /// 处理可视化消息 - 解析后委托给制造商专用处理器
        /// @author zzy
        /// </summary>
        private async Task<MqttHandleResult> HandleVisualizationMessageAsync(string robotManufacturer, string robotSerialNumber, string payload)
        {
            try
            {
                // 第一步：通过工厂按制造商解析可视化数据
                var visualization = _visualizationParserFactory.ParseVisualization(robotManufacturer, payload);
                if (visualization == null)
                    return MqttHandleResult.CreateFailure("可视化数据解析失败");

                // 第二步：headerId去重判断
                if (!await IsNewHeaderIdAsync("visualization", robotManufacturer, robotSerialNumber, visualization.HeaderId))
                    return MqttHandleResult.CreateFailure($"可视化消息HeaderId: {visualization.HeaderId} 消息过期");

                // 第三步：委托给制造商专用处理器处理数据
                var processor = _visualizationProcessorFactory.GetProcessor(robotManufacturer);
                var processResult = await processor.ProcessAsync(robotManufacturer, robotSerialNumber, visualization);

                if (!processResult.Success)
                {
                    _logger.LogWarning(
                        "可视化数据处理失败: Robot={Manufacturer}:{SerialNumber}, Message={Message}",
                        robotManufacturer, robotSerialNumber, processResult.Message);
                }

                return MqttHandleResult.CreateSuccess(
                    $"可视化消息处理完成: {robotManufacturer}:{robotSerialNumber}");
            }
            catch (Exception ex)
            {
                return MqttHandleResult.CreateFailure($"处理可视化消息失败: {ex.Message}", ex);
            }
        }

        /// <summary>
        /// 从MQTT消息中提取载荷，并将Unicode转义序列转换为正常UTF-8字符
        /// </summary>
        private static string ExtractPayload(MQTTnet.MqttApplicationMessage message)
        {
            if (message.PayloadSegment.Array is not { } arr)
            {
                return string.Empty;
            }

            // 1. 将字节数组解码为UTF-8字符串
            var rawString = System.Text.Encoding.UTF8.GetString(
                arr, 
                message.PayloadSegment.Offset, 
                message.PayloadSegment.Count);

            // 2. 将Unicode转义序列(\uXXXX)转换为正常字符
            return System.Text.RegularExpressions.Regex.Replace(
                rawString,
                @"\\u([0-9a-fA-F]{4})",
                match => ((char)Convert.ToInt32(match.Groups[1].Value, 16)).ToString());
        }

        private string BuildHeaderHashKey(string robotManufacturer, string robotSN)
        {
            return $"{_settings.Redis.KeyPrefixes.Robot}:{robotManufacturer}:{robotSN}:{_settings.Redis.KeyPrefixes.MqttHeaderSuffix}";
        }

        // 基于Redis按机器人维度检查并更新headerId
        private async Task<bool> IsNewHeaderIdAsync(string topic, string robotManufacturer,string robotSN, int newHeaderId)
        {
            try
            {
                var redisDb = _redis.GetDatabase();
                var key = BuildHeaderHashKey(robotManufacturer,robotSN);
                var field = topic.ToLowerInvariant();

                var currentValue = await redisDb.HashGetAsync(key, field);
                var currentHeaderId = 0;
                if (currentValue.HasValue && int.TryParse(currentValue.ToString(), out var parsed))
                {
                    currentHeaderId = parsed;
                }
                
                if (newHeaderId > currentHeaderId)
                {
                    await redisDb.HashSetAsync(key, field, newHeaderId);
                    return true;
                }

                return false;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "检查并更新机器人 {RobotCode} 的 {Topic} headerId 失败", robotManufacturer + robotSN, topic);
                return false;
            }
        }

        // 将指定机器人的所有topic headerId重置为0
        public async Task ResetHeaderIdsAsync( string robotManufacturer,string robotSN)
        {
            try
            {
                var redisDb = _redis.GetDatabase();
                var key = BuildHeaderHashKey(robotManufacturer,robotSN);
                var entries = new HashEntry[]
                {
                    new("connection", 0),
                    new("state", 0),
                    new("factsheet", 0),
                    new("visualization", 0)
                };
                await redisDb.HashSetAsync(key, entries);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "重置机器人 {RobotCode} 的 headerId 失败", robotManufacturer + robotSN );
            }
        }      
        /// <summary>
        /// 根据状态信息判断Robot状态
        /// </summary>
        private RobotStatus DetermineRobotStatus(State stateInfo)
        {
            // 判断是否有致命错误
            var hasFatalError = stateInfo.Errors.Any(e =>
                e.ErrorLevel?.Equals("FATAL", StringComparison.OrdinalIgnoreCase) == true);

            if (hasFatalError)
                return RobotStatus.Error;

            // 机器人必须已停止行驶
            if (stateInfo.Driving)
            {
                return RobotStatus.Busy;
            }

            // NodeStates和EdgeStates必须为空（所有路径已走完）
            else if (stateInfo.NodeStates != null && stateInfo.NodeStates.Count > 0)
            {
                return RobotStatus.Busy;
            }
            else if (stateInfo.EdgeStates != null && stateInfo.EdgeStates.Count > 0)
            {
                return RobotStatus.Busy;
            }

            // 所有动作必须已完成（不存在未完成的动作）
            else if (stateInfo.ActionStates != null && stateInfo.ActionStates.Count > 0)
            {
                var hasUnfinishedAction = stateInfo.ActionStates.Any(a =>
                    a.ActionStatus != "FINISHED" && a.ActionStatus != "FAILED");
                if (hasUnfinishedAction)
                {
                    return RobotStatus.Busy;
                }
            }

            // 默认空闲
            return RobotStatus.Idle;
        }

        /// <summary>
        /// 解析操作模式
        /// @author zzy
        /// </summary>
        private static OperatingMode ParseOperatingMode(string? mode)
        {
            return mode?.ToUpperInvariant() switch
            {
                "AUTOMATIC" => OperatingMode.Automatic,
                "SEMIAUTOMATIC" => OperatingMode.Semiautomatic,
                "MANUAL" => OperatingMode.Manual,
                "SERVICE" => OperatingMode.Service,
                "TEACHIN" => OperatingMode.Teachin,
                _ => OperatingMode.Automatic
            };
        }

        /// <summary>
        /// 根据机器人 LastNode 在已下发且已发送的路径中释放已执行节点/边锁（不释放 LastNode 本身）
        /// 释放完成后，续期仅覆盖仍可能占用的路径锁
        /// @author zzy
        /// @date 2026-03-05
        /// </summary>
        private async Task ReleaseExecutedLocksBeforeLastNodeAsync(
            Guid robotId,
            string robotManufacturer,
            string robotSerialNumber,
            string? lastNodeId)
        {
            if (string.IsNullOrWhiteSpace(lastNodeId))
            {
                return;
            }

            try
            {
                var endpoints = _redis.GetEndPoints();
                if (endpoints.Length == 0)
                {
                    return;
                }

                var server = _redis.GetServer(endpoints.First());
                var db = _redis.GetDatabase();
                var pattern = $"{_settings.Redis.KeyPrefixes.VdaPath}:{robotId}:*";
                var cacheKeys = server.Keys(pattern: pattern).ToArray();

                if (cacheKeys.Length == 0)
                {
                    return;
                }

                VdaSegmentedPathCache? activeCache = null;
                foreach (var cacheKey in cacheKeys)
                {
                    var keyText = cacheKey.ToString();
                    if (!IsVdaPathCachePayloadKey(keyText, robotId))
                    {
                        continue;
                    }

                    var cacheData = await db.StringGetAsync(cacheKey);
                    if (cacheData.IsNullOrEmpty)
                    {
                        continue;
                    }

                    VdaSegmentedPathCache? candidate;
                    try
                    {
                        candidate = JsonSerializer.Deserialize<VdaSegmentedPathCache>(cacheData.ToString());
                    }
                    catch (Exception ex) when (ex is JsonException or NotSupportedException)
                    {
                        _logger.LogDebug("[路径锁] 跳过无法反序列化的路径缓存Key: {CacheKey}", keyText);
                        continue;
                    }

                    if (candidate == null)
                    {
                        continue;
                    }

                    if (activeCache == null || candidate.CreatedAt > activeCache.CreatedAt)
                    {
                        activeCache = candidate;
                    }
                }

                if (activeCache == null || string.IsNullOrWhiteSpace(activeCache.MapCode))
                {
                    return;
                }

                var sentSegments = activeCache.JunctionSegments
                    .SelectMany(j => j.ResourceSegments)
                    .Where(r => r.IsSent)
                    .SelectMany(r => r.Segments)
                    .ToList();

                if (sentSegments.Count == 0)
                {
                    return;
                }

                var executedSegments = new List<PathSegmentWithCode>();
                var matchedLastNode = false;

                foreach (var segment in sentSegments)
                {
                    if (string.IsNullOrWhiteSpace(segment.FromNodeCode) || string.IsNullOrWhiteSpace(segment.ToNodeCode))
                    {
                        continue;
                    }

                    if (string.Equals(segment.FromNodeCode, lastNodeId, StringComparison.OrdinalIgnoreCase))
                    {
                        matchedLastNode = true;
                        break;
                    }

                    executedSegments.Add(segment);

                    if (string.Equals(segment.ToNodeCode, lastNodeId, StringComparison.OrdinalIgnoreCase))
                    {
                        matchedLastNode = true;
                        break;
                    }
                }

                if (!matchedLastNode || executedSegments.Count == 0)
                {
                    return;
                }

                var nodeCodesToRelease = new HashSet<string>(StringComparer.OrdinalIgnoreCase);
                var edgeCodesToRelease = new HashSet<string>(StringComparer.OrdinalIgnoreCase);
                foreach (var segment in executedSegments)
                {
                    nodeCodesToRelease.Add(segment.FromNodeCode);
                    nodeCodesToRelease.Add(segment.ToNodeCode);

                    if (!string.IsNullOrWhiteSpace(segment.EdgeCode))
                    {
                        edgeCodesToRelease.Add(segment.EdgeCode);
                    }
                }

                var graph = await _agvPathService.GetOrBuildGraphAsync(activeCache.MapId);
                if (graph != null)
                {
                    var nodeLookup = SweptAreaCoverageResolver.BuildNodeLookup(graph);
                    var edgePolylineLookup = SweptAreaCoverageResolver.BuildEdgePolylineLookup(graph);
                    var centerLine = SweptAreaCoverageResolver.BuildPolylineFromSegments(
                        executedSegments,
                        nodeLookup,
                        edgePolylineLookup);
                    if (centerLine.Count > 0)
                    {
                        var basicCache = await _robotCacheService.GetBasicAsync(robotManufacturer, robotSerialNumber);
                        var dimensions = SweptAreaCoverageResolver.ResolveDimensions(
                            basicCache?.Width,
                            basicCache?.Length,
                            basicCache?.SafetyDistance,
                            basicCache?.CoordinateScale ?? 1d);
                        var radius = SweptAreaCoverageResolver.CalculateSweepRadius(
                            dimensions.Length,
                            dimensions.Width,
                            dimensions.SafetyDistance);

                        var coverage = SweptAreaCoverageResolver.ExpandFromGraph(graph, centerLine, radius);
                        nodeCodesToRelease.UnionWith(coverage.NodeCodes);
                        edgeCodesToRelease.UnionWith(coverage.EdgeCodes);
                    }
                }

                nodeCodesToRelease.Remove(lastNodeId);

                if (nodeCodesToRelease.Count == 0 && edgeCodesToRelease.Count == 0)
                {
                    return;
                }

                var releasedCount = await _agvPathService.ReleasePathSegmentsAsync(
                    activeCache.MapCode,
                    nodeCodesToRelease,
                    edgeCodesToRelease,
                    robotId);

                if (releasedCount > 0)
                {
                    _logger.LogDebug(
                        "[路径锁] 已按LastNode释放执行过锁: RobotId={RobotId}, LastNode={LastNode}, ReleasedCount={Count}",
                        robotId, lastNodeId, releasedCount);
                }
            }
            catch (Exception ex)
            {
                _logger.LogWarning(ex,
                    "[路径锁] 按LastNode释放执行过锁失败: RobotId={RobotId}, LastNode={LastNode}",
                    robotId, lastNodeId);
            }
        }

        /// <summary>
        /// 判断是否为VDA路径缓存主体Key（...:{robotId}:{taskId}:{subTaskId}）。
        /// </summary>
        private static bool IsVdaPathCachePayloadKey(string keyText, Guid robotId)
        {
            if (string.IsNullOrWhiteSpace(keyText))
            {
                return false;
            }

            var parts = keyText.Split(':', StringSplitOptions.RemoveEmptyEntries | StringSplitOptions.TrimEntries);
            if (parts.Length < 3)
            {
                return false;
            }

            if (!Guid.TryParse(parts[^3], out var keyRobotId) || keyRobotId != robotId)
            {
                return false;
            }

            return Guid.TryParse(parts[^2], out _) && Guid.TryParse(parts[^1], out _);
        }

        /// <summary>
        /// 处理NewBaseRequest请求，当车辆快到当前段终点时，调用任务服务发送下一段路径指令
        /// @author zzy
        /// </summary>
        /// <param name="robotManufacturer">机器人制造商</param>
        /// <param name="robotSerialNumber">机器人序列号</param>
        private async Task HandleNewBaseRequestAsync(string robotManufacturer, string robotSerialNumber)
        {
            try
            {
                // 使用Scope解析协议服务，避免循环依赖
                using var scope = _scopeFactory.CreateScope();
                var protocolServiceFactory = scope.ServiceProvider.GetRequiredService<IProtocolServiceFactory>();
                var protocol = protocolServiceFactory.GetService(ProtocolType.VDA);

                // 判断是否所有段都已发送完毕，若是则跳过发送
                // @author zzy
                var allSent = await protocol.IsAllSegmentsSentAsync(robotManufacturer, robotSerialNumber);
                if (allSent)
                {
                    return;
                }

                // 判断机器人当前是否行驶在目前已下发路径集合中的最后一段路径上
                // 若不在最后一段上，则跳过发送（避免在已下发路径中间段时重复请求）
                // @author zzy
                var onLastSentSegment = await protocol.IsOnLastSentSegmentAsync(robotManufacturer, robotSerialNumber);
                if (!onLastSentSegment)
                {
                    return;
                }

                // 调用任务服务发送下一段路径指令
                await protocol.SendNextSegmentAsync(robotManufacturer, robotSerialNumber);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "处理NewBaseRequest失败: Robot={Manufacturer}:{SerialNumber}",
                    robotManufacturer, robotSerialNumber);
            }
        }

        /// <summary>
        /// 判断机器人是否存在执行中的主任务，且该主任务包含执行中的子任务
        /// @author zzy
        /// </summary>
        /// <param name="robotManufacturer">机器人制造商</param>
        /// <param name="robotSerialNumber">机器人序列号</param>
        /// <returns>存在则返回true，否则返回false</returns>
        private async Task<bool> HasInProgressTaskWithInProgressSubTaskAsync(
            string robotManufacturer,
            string robotSerialNumber)
        {
            var basicInfo = await _robotCacheService.GetBasicAsync(robotManufacturer, robotSerialNumber);
            if (basicInfo == null || !Guid.TryParse(basicInfo.RobotId, out var robotId))
            {
                return false;
            }

            using var scope = _scopeFactory.CreateScope();
            var taskRepository = scope.ServiceProvider.GetRequiredService<IRobotTaskRepository>();
            var robotTasks = await taskRepository.GetByRobotIdAsync(robotId);

            return robotTasks.Any(task =>
                task.Status == Rcs.Domain.Entities.TaskStatus.InProgress
                && task.SubTasks.Any(subTask => subTask.Status == Rcs.Domain.Entities.TaskStatus.InProgress));
        }

        /// <summary>
        /// 尝试创建自动节点校准工单。
        /// 当状态数据表明机器人在缓存中无匹配节点但具有有效坐标位置时触发。
        /// </summary>
        /// <param name="robotManufacturer">MQTT 主题中的机器人制造商。</param>
        /// <param name="robotSerialNumber">MQTT 主题中的机器人序列号。</param>
        /// <param name="stateInfo">解析后的 VDA5050 State 消息载荷。</param>
        /// <param name="robotStatus">当前推导的机器人状态。</param>
        /// <param name="hasInProgressTask">是否存在进行中的任务且包含进行中的子任务。</param>
        private async Task TryCreateAutoNodeCalibrationOrderAsync(
            string robotManufacturer,
            string robotSerialNumber,
            State stateInfo,
            RobotStatus robotStatus,
            bool hasInProgressTask)
        {
            try
            {
                if (robotStatus != RobotStatus.Idle || stateInfo.Driving || stateInfo.Paused == true || hasInProgressTask)
                {
                    return;
                }

                var locationCache = await _robotCacheService.GetLocationAsync(robotManufacturer, robotSerialNumber);
                if (locationCache?.NodeId.HasValue == true)
                {
                    return;
                }

                using var scope = _scopeFactory.CreateScope();
                var robotRepository = scope.ServiceProvider.GetRequiredService<IRobotRepository>();
                var mapNodeRepository = scope.ServiceProvider.GetRequiredService<IMapNodeRepository>();
                var mapEdgeRepository = scope.ServiceProvider.GetRequiredService<IMapEdgeRepository>();
                var mqttClientService = scope.ServiceProvider.GetRequiredService<IMqttClientService>();

                var robot = await robotRepository.GetByManufacturerAndSerialNumberAsync(robotManufacturer, robotSerialNumber);
                if (robot == null || !robot.Active || !robot.CurrentMapCodeId.HasValue || !robot.Radius.HasValue || robot.Radius < 0)
                {
                    return;
                }

                var hasStatePosition = stateInfo.AgvPosition != null;
                var rawX = hasStatePosition ? stateInfo.AgvPosition!.X : locationCache?.X;
                var rawY = hasStatePosition ? stateInfo.AgvPosition!.Y : locationCache?.Y;
                if (!rawX.HasValue || !rawY.HasValue)
                {
                    return;
                }

                var coordinateScale = robot.CoordinateScale > 0 ? robot.CoordinateScale : 1d;
                var currentX = hasStatePosition ? rawX.Value * coordinateScale : rawX.Value;
                var currentY = hasStatePosition ? rawY.Value * coordinateScale : rawY.Value;
                var mapId = locationCache?.MapId ?? robot.CurrentMapCodeId.Value;

                var mapNodes = (await mapNodeRepository.GetByMapIdAsync(mapId))
                    .ToDictionary(node => node.NodeId);
                if (mapNodes.Count == 0)
                {
                    return;
                }

                var mapEdges = await mapEdgeRepository.GetByMapIdAsync(mapId);
                if (!TryFindClosestPathAndNodeWithinRadius(
                        mapEdges,
                        mapNodes,
                        currentX,
                        currentY,
                        robot.Radius.Value,
                        out var closestEdge,
                        out var closestNode,
                        out var closestPathDistance))
                {
                    return;
                }

                // 构建 VDA5050 Order，直接下发两个节点的移动订单
                var targetNode = closestNode;
                var mapCode = mapId.ToString();
                var orderId = $"NCAL-{Guid.NewGuid():N}";
                robot.HeaderId++;

                // 起点节点：当前坐标，NodeId 为虚拟值
                var virtualStartNodeId = $"NCAL-START-{Guid.NewGuid().ToString("N")[..6]}";
                var startNode = new Node
                {
                    NodeId = virtualStartNodeId,
                    SequenceId = 0,
                    Released = true,
                    NodeDescription = "Auto node calibration virtual start",
                    NodePosition = new NodePosition
                    {
                        X = currentX / coordinateScale,
                        Y = currentY / coordinateScale,
                        Theta = stateInfo.AgvPosition?.Theta ?? 0d,
                        MapId = mapCode,
                        AllowedDeviationXY = closestNode.MaxCoordinateOffset ?? 0d
                    },
                    Actions = new List<Domain.Models.VDA5050.Action>()
                };

                // 终点节点：目标节点（真实节点）
                var endNode = new Node
                {
                    NodeId = targetNode.NodeCode,
                    SequenceId = 2,
                    Released = true,
                    NodeDescription = "Auto node calibration target",
                    NodePosition = new NodePosition
                    {
                        X = targetNode.X / coordinateScale,
                        Y = targetNode.Y / coordinateScale,
                        Theta = stateInfo.AgvPosition?.Theta ?? 0d,
                        MapId = mapCode,
                        AllowedDeviationXY = closestNode.MaxCoordinateOffset ?? 0d
                    },
                    Actions = new List<Domain.Models.VDA5050.Action>()
                };

                // 连接起点和终点的边
                var edge = new Edge
                {
                    EdgeId = $"NCAL-EDGE-{Guid.NewGuid().ToString("N")[..6]}",
                    SequenceId = 1,
                    Released = true,
                    StartNodeId = virtualStartNodeId,
                    EndNodeId = targetNode.NodeCode,
                    MaxSpeed = closestEdge.MaxSpeed,
                    Orientation = 0
                };

                var order = new Domain.Models.VDA5050.Order
                {
                    HeaderId = (int)robot.HeaderId,
                    Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffzzz"),
                    Version = robot.ProtocolVersion,
                    Manufacturer = robot.RobotManufacturer,
                    SerialNumber = robot.RobotSerialNumber,
                    ZoneSetId = mapCode,
                    OrderId = orderId,
                    OrderUpdateId = 0,
                    Nodes = new List<Node> { startNode, endNode },
                    Edges = new List<Edge> { edge }
                };

                // 通过 MQTT 下发订单
                await mqttClientService.PublishOrderAsync(
                    robot.ProtocolName,
                    robot.ProtocolVersion,
                    robot.RobotManufacturer,
                    robot.RobotSerialNumber,
                    order);

                // 持久化 HeaderId
                await robotRepository.SaveChangesAsync();

                _logger.LogInformation(
                    "[Auto Node Calibration] Sent VDA5050 order. Robot={Manufacturer}:{SerialNumber}, OrderId={OrderId}, Edge={EdgeCode}, TargetNode={NodeCode}, Distance={Distance:F3}, Radius={Radius:F3}",
                    robotManufacturer,
                    robotSerialNumber,
                    orderId,
                    closestEdge.EdgeCode,
                    targetNode.NodeCode,
                    closestPathDistance,
                    robot.Radius.Value);
            }
            catch (Exception ex)
            {
                _logger.LogWarning(
                    ex,
                    "[Auto Node Calibration] Failed to create order. Robot={Manufacturer}:{SerialNumber}",
                    robotManufacturer,
                    robotSerialNumber);
            }
        }

        /// <summary>
        /// 通过点到线段的直线距离查找最近的地图边，并返回最近的端点节点。
        /// </summary>
        /// <param name="edges">候选地图边集合。</param>
        /// <param name="nodeLookup">按节点 ID 索引的节点查找字典。</param>
        /// <param name="x">当前 X 坐标。</param>
        /// <param name="y">当前 Y 坐标。</param>
        /// <param name="maxDistance">允许的最大边距离。</param>
        /// <param name="closestEdge">输出参数：最近的边。</param>
        /// <param name="closestNode">输出参数：最近的端点节点。</param>
        /// <param name="closestDistance">输出参数：最短的点到边距离。</param>
        /// <returns>若在距离阈值内找到有效的边-节点对则返回 <c>true</c>，否则返回 <c>false</c>。</returns>
        private static bool TryFindClosestPathAndNodeWithinRadius(
            IEnumerable<MapEdge> edges,
            IReadOnlyDictionary<Guid, MapNode> nodeLookup,
            double x,
            double y,
            double maxDistance,
            out MapEdge closestEdge,
            out MapNode closestNode,
            out double closestDistance)
        {
            closestEdge = null!;
            closestNode = null!;
            closestDistance = double.MaxValue;

            foreach (var edge in edges)
            {
                if (!nodeLookup.TryGetValue(edge.FromNode, out var fromNode)
                    || !nodeLookup.TryGetValue(edge.ToNode, out var toNode))
                {
                    continue;
                }

                var edgeDistance = CalculatePointToSegmentDistance(
                    x,
                    y,
                    fromNode.X,
                    fromNode.Y,
                    toNode.X,
                    toNode.Y);
                if (edgeDistance >= closestDistance)
                {
                    continue;
                }

                closestDistance = edgeDistance;
                closestEdge = edge;

                var fromDistance = CalculatePointDistance(x, y, fromNode.X, fromNode.Y);
                var toDistance = CalculatePointDistance(x, y, toNode.X, toNode.Y);
                closestNode = fromDistance <= toDistance ? fromNode : toNode;
            }

            return closestEdge != null
                   && closestNode != null
                   && closestDistance <= maxDistance;
        }

        /// <summary>
        /// 计算两点之间的直线距离。
        /// </summary>
        /// <param name="x1">第一个点的 X 坐标。</param>
        /// <param name="y1">第一个点的 Y 坐标。</param>
        /// <param name="x2">第二个点的 X 坐标。</param>
        /// <param name="y2">第二个点的 Y 坐标。</param>
        /// <returns>欧几里得距离。</returns>
        private static double CalculatePointDistance(double x1, double y1, double x2, double y2)
        {
            var dx = x1 - x2;
            var dy = y1 - y2;
            return Math.Sqrt(dx * dx + dy * dy);
        }

        /// <summary>
        /// 计算点到线段的最短直线距离。
        /// </summary>
        /// <param name="px">点的 X 坐标。</param>
        /// <param name="py">点的 Y 坐标。</param>
        /// <param name="x1">线段起点 X 坐标。</param>
        /// <param name="y1">线段起点 Y 坐标。</param>
        /// <param name="x2">线段终点 X 坐标。</param>
        /// <param name="y2">线段终点 Y 坐标。</param>
        /// <returns>点到线段的最短距离。</returns>
        private static double CalculatePointToSegmentDistance(
            double px,
            double py,
            double x1,
            double y1,
            double x2,
            double y2)
        {
            var dx = x2 - x1;
            var dy = y2 - y1;
            if (Math.Abs(dx) < double.Epsilon && Math.Abs(dy) < double.Epsilon)
            {
                return CalculatePointDistance(px, py, x1, y1);
            }

            var t = ((px - x1) * dx + (py - y1) * dy) / (dx * dx + dy * dy);
            t = Math.Clamp(t, 0d, 1d);

            var projectionX = x1 + t * dx;
            var projectionY = y1 + t * dy;
            return CalculatePointDistance(px, py, projectionX, projectionY);
        }

        /// <summary>
        /// 尝试处理VDA5050订单完成判断
        /// 从State消息中提取关键字段，委托给协议服务进行完成判断
        /// 失败不影响正常的State消息处理流程
        /// @author zzy
        /// </summary>
        /// <param name="robotManufacturer">机器人制造商</param>
        /// <param name="robotSerialNumber">机器人序列号</param>
        /// <param name="stateInfo">VDA5050 State消息数据</param>
        private async Task TryHandleOrderCompletionAsync(
            string robotManufacturer,
            string robotSerialNumber,
            State stateInfo)
        {
            try
            {
                using var scope = _scopeFactory.CreateScope();
                var protocolServiceFactory = scope.ServiceProvider.GetRequiredService<IProtocolServiceFactory>();
                var protocol = protocolServiceFactory.GetService(ProtocolType.VDA);

                var completed = await protocol.TryCompleteOrderFromStateAsync(
                    robotManufacturer,
                    robotSerialNumber,
                    stateInfo);

                if (completed)
                {
                    _logger.LogInformation(
                        "[VDA完成] 自动检测到订单完成: Robot={Manufacturer}:{SerialNumber}, OrderId={OrderId}",
                        robotManufacturer, robotSerialNumber, stateInfo.OrderId);
                }
            }
            catch (Exception ex)
            {
                // 完成判断失败不应影响正常的State消息处理流程
                _logger.LogError(ex,
                    "[VDA完成] 订单完成判断异常: Robot={Manufacturer}:{SerialNumber}, OrderId={OrderId}",
                    robotManufacturer, robotSerialNumber, stateInfo.OrderId);
            }
        }
    }
}