zengzy / hhrcs_service · Files

using Microsoft.Extensions.DependencyInjection;
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using Microsoft.Extensions.Options;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
using Rcs.Application.Services.Protocol;
using Rcs.Application.Shared;
using Rcs.Domain.Entities;
using Rcs.Domain.Enums;
using Rcs.Domain.Models.VDA5050;
using Rcs.Domain.Repositories;
using Rcs.Domain.Settings;
using Rcs.Infrastructure.PathFinding.Services;
using StackExchange.Redis;
using System.Text.Json;
using Newtonsoft.Json;
using TaskStatus = Rcs.Domain.Entities.TaskStatus;

namespace Rcs.Infrastructure.Services
{
    /// <summary>
    /// 后台任务执行服务 - 将已分配的任务下发给空闲机器人执行
    /// @author zzy
    /// </summary>
    public class TaskExecutionBackgroundService : BackgroundService, ITaskExecutionService
    {
        private readonly ILogger<TaskExecutionBackgroundService> _logger;
        private readonly IServiceScopeFactory _serviceScopeFactory;
        private readonly IRobotCacheService _robotCacheService;
        private readonly IAgvPathService _agvPathService;
        private readonly IConnectionMultiplexer _redis;
        private readonly AppSettings _settings;
        private readonly TimeSpan _executionInterval = TimeSpan.FromSeconds(2);
        private const int MaxAssignedTasksPerCycle = 10;
        

        public TaskExecutionBackgroundService(
            ILogger<TaskExecutionBackgroundService> logger,
            IServiceScopeFactory serviceScopeFactory,
            IRobotCacheService robotCacheService,
            IAgvPathService agvPathService,
            IConnectionMultiplexer redis,
            IOptions<AppSettings> settings)
        {
            _logger = logger;
            _serviceScopeFactory = serviceScopeFactory;
            _robotCacheService = robotCacheService;
            _agvPathService = agvPathService;
            _redis = redis;
            _settings = settings.Value;
        }

        /// <summary>
        /// 后台服务执行入口
        /// @author zzy
        /// </summary>
        protected override async Task ExecuteAsync(CancellationToken stoppingToken)
        {
            _logger.LogInformation("[任务执行] 后台任务执行服务已启动");

            while (!stoppingToken.IsCancellationRequested)
            {
                try
                {
                    await ExecuteTasksAsync(stoppingToken);
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "[任务执行] 执行过程发生异常");
                }

                await Task.Delay(_executionInterval, stoppingToken);
            }

            _logger.LogInformation("[任务执行] 后台任务执行服务已停止");
        }

        /// <summary>
        /// 执行一次任务执行检查
        /// @author zzy
        /// </summary>
        public async Task<TaskExecutionResult> ExecuteTasksAsync(CancellationToken cancellationToken = default)
        {
            using var scope = _serviceScopeFactory.CreateScope();
            var taskRepo = scope.ServiceProvider.GetRequiredService<IRobotTaskRepository>();
            var robotRepo = scope.ServiceProvider.GetRequiredService<IRobotRepository>();

            // 1. 获取已分配和执行中的任务（执行中的任务可能有待执行的子任务）
            var assignedTasks = await taskRepo.GetByStatusAsync(TaskStatus.Assigned, cancellationToken);
            var inProgressTasks = await taskRepo.GetByStatusAsync(TaskStatus.InProgress, cancellationToken);
            
            var activeTasks = assignedTasks
                .Concat(inProgressTasks)
                .Where(t => t.RobotId.HasValue)
                .Where(t => t.SubTasks.Any())
                .ToList();

            if (!activeTasks.Any())
            {
                return new TaskExecutionResult { Success = true, ExecutedCount = 0, Message = "无待执行任务" };
            }

            var tasksToExecute = await BuildExecutionQueueAsync(activeTasks, robotRepo, taskRepo, cancellationToken);
            _logger.LogInformation("执行队列{queue}",JsonConvert.SerializeObject(tasksToExecute.Select(t => t.ShelfCode)));
            if (!tasksToExecute.Any())
            {
                return new TaskExecutionResult { Success = true, ExecutedCount = 0, Message = "无待执行任务" };
            }

            int executedCount = 0;

            foreach (var task in tasksToExecute)
            {
                if (!task.RobotId.HasValue) continue;

                // 2. 获取机器人信息
                var robot = await robotRepo.GetByIdFullDataAsync(task.RobotId.Value, cancellationToken);
                if (robot == null)
                {
                    _logger.LogWarning("[任务执行] 任务 {TaskCode} 对应的机器人不存在", task.TaskCode);
                    continue;
                }

                if (!robot.IsRobotAvailable())
                {
                    _logger.LogDebug("[任务执行] 机器人 {RobotCode} 当前不可用", robot.RobotCode);
                    continue;
                }

                // 4. 检查机器人是否有执行中的任务
                var hasInProgressTask = await HasInProgressTaskAsync(robot.RobotId, task.TaskId, taskRepo, cancellationToken);
                
                if (hasInProgressTask)
                {
                    _logger.LogDebug("[任务执行] 机器人 {RobotCode} 存在执行中任务", robot.RobotCode);
                    continue;
                }

                // 5. 根据协议类型执行任务
                var success = await ExecuteTaskByProtocolAsync(task, robot, taskRepo, scope, cancellationToken);
                if (success)
                {
                    executedCount++;
                }
            }

            return new TaskExecutionResult
            {
                Success = true,
                ExecutedCount = executedCount,
                Message = $"本次执行完成，已下发 {executedCount} 个任务"
            };
        }

        /// <summary>
        /// 构建执行队列：按机器人分组后，为每台机器人挑选一个最优候选任务。
        /// 候选任务按“当前位置到目标点的可行路径总代价”排序（不进行路径重合率评估）。
        /// </summary>
        /// <param name="activeTasks">当前活动任务集合（Assigned + InProgress）</param>
        /// <param name="robotRepo">机器人仓储</param>
        /// <param name="taskRepo">任务仓储</param>
        /// <param name="cancellationToken">取消令牌</param>
        /// <returns>本轮待执行任务列表</returns>
        private async Task<List<RobotTask>> BuildExecutionQueueAsync(
            IEnumerable<RobotTask> activeTasks,
            IRobotRepository robotRepo,
            IRobotTaskRepository taskRepo,
            CancellationToken cancellationToken)
        {
            var selectedCandidates = new List<TaskCandidate>();
            var mapGraphCache = new Dictionary<Guid, PathGraph?>();

            // 按机器人分组，每个机器人每轮最多选择一个“下一下发任务”
            var taskGroups = activeTasks
                .Where(t => t.RobotId.HasValue)
                .GroupBy(t => t.RobotId!.Value);

            foreach (var taskGroup in taskGroups)
            {
                var robot = await robotRepo.GetByIdFullDataAsync(taskGroup.Key, cancellationToken);
                if (robot == null)
                {
                    continue;
                }
                // 判断机器人是否仍在执行：只看“执行中子任务”，不依赖主任务状态
                var hasInProgressSubTask = taskGroup.Any(t =>
                    t.SubTasks.Any(st => st.Status == TaskStatus.InProgress));
                if (hasInProgressSubTask)
                {
                    continue;
                }

                var candidateTasks = new List<TaskCandidate>();
                foreach (var task in taskGroup)
                {
                    var taskWithDetails = await taskRepo.GetByIdWithDetailsAsync(task.TaskId, cancellationToken);
                    if (taskWithDetails == null)
                    {
                        continue;
                    }

                    if (!taskWithDetails.SubTasks.Any() && taskWithDetails.Status == TaskStatus.InProgress)
                    {
                        continue;
                    }

                    var nextSubTask = taskWithDetails.GetNextExecutableSubTask();
                    if (taskWithDetails.SubTasks.Any() && nextSubTask == null)
                    {
                        continue;
                    }

                    var pathCost = await CalculatePathCostToTaskAsync(
                        robot,
                        taskWithDetails,
                        nextSubTask,
                        mapGraphCache,
                        cancellationToken);
                    if (pathCost == double.MaxValue)
                    {
                        continue;
                    }

                    candidateTasks.Add(new TaskCandidate(taskWithDetails, pathCost));
                }

                if (!candidateTasks.Any())
                {
                    continue;
                }

                // 仅按路径代价、创建时间与 TaskId 做稳定排序
                var selectedTask = candidateTasks
                    .OrderBy(c => c, TaskCandidateComparer.Instance)
                    .FirstOrDefault();

                if (selectedTask != null)
                {
                    // CalculatePathAsync 会更新机器人规划路径，需确保最终状态与选中任务一致。
                    if (candidateTasks.Count > 1)
                    {
                        var selectedNextSubTask = selectedTask.Task.GetNextExecutableSubTask();
                        await CalculatePathCostToTaskAsync(
                            robot,
                            selectedTask.Task,
                            selectedNextSubTask,
                            mapGraphCache,
                            cancellationToken);
                    }

                    selectedCandidates.Add(selectedTask);
                }
            }

            return selectedCandidates
                .OrderBy(c => c, TaskCandidateComparer.Instance)
                .Select(c => c.Task)
                .Take(MaxAssignedTasksPerCycle)
                .ToList();
        }

        /// <summary>
        /// 计算机器人到候选任务目标点的可行路径总代价。
        /// 若路径不可行或关键数据缺失，返回 <see cref="double.MaxValue"/>。
        /// </summary>
        /// <param name="robot">机器人实体</param>
        /// <param name="task">候选任务</param>
        /// <param name="nextSubTask">候选任务的下一子任务</param>
        /// <param name="mapGraphCache">地图图缓存，避免重复加载</param>
        /// <param name="cancellationToken">取消令牌</param>
        /// <returns>可行路径总代价</returns>
        private async Task<double> CalculatePathCostToTaskAsync(
            Robot robot,
            RobotTask task,
            RobotSubTask? nextSubTask,
            IDictionary<Guid, PathGraph?> mapGraphCache,
            CancellationToken cancellationToken)
        {
            var targetNodeId = nextSubTask?.EndNodeId
                ?? task.BeginLocation?.MapNode?.NodeId
                ?? task.EndLocation?.MapNode?.NodeId;
            if (!targetNodeId.HasValue || targetNodeId.Value == Guid.Empty)
            {
                return double.MaxValue;
            }

            var mapId = nextSubTask?.EndNode?.MapId
                ?? task.BeginLocation?.MapNode
                ?.MapId
                ?? task.EndLocation?.MapNode
                ?.MapId
                ?? robot.CurrentMapCodeId;
            if (!mapId.HasValue)
            {
                return double.MaxValue;
            }

            if (!mapGraphCache.TryGetValue(mapId.Value, out var graph))
            {
                graph = await _agvPathService.GetOrBuildGraphAsync(mapId.Value);
                mapGraphCache[mapId.Value] = graph;
            }

            if (graph == null || !graph.Nodes.TryGetValue(targetNodeId.Value, out var targetPathNode))
            {
                return double.MaxValue;
            }

            var startNodeId = ResolveStartNodeId(robot, graph);
            if (!startNodeId.HasValue)
            {
                return double.MaxValue;
            }

            var request = new PathRequest
            {
                RobotId = robot.RobotId,
                MapId = mapId.Value,
                StartNodeId = startNodeId.Value,
                EndNodeId = targetNodeId.Value,
                CurrentTheta = robot.CurrentTheta ?? 0d,
                IsLoaded = !string.IsNullOrWhiteSpace(task.ContainerID),
                Priority = task.Priority,
                BatteryLevel = robot.BatteryLevel ?? 100,
                MovementType = robot.MovementType,
                ForkRadOffsets = robot.ForkRadOffset,
                RequiredEndRad = targetPathNode.Theta
            };

            try
            {
                var pathResult = await _agvPathService.CalculatePathAsync(request, cancellationToken);
                if (!pathResult.Success)
                {
                    _logger.LogDebug(
                        "[任务执行] 候选任务不可达，跳过: RobotCode={RobotCode}, TaskCode={TaskCode}, Reason={Reason}",
                        robot.RobotCode,
                        task.TaskCode,
                        pathResult.ErrorMessage);
                    return double.MaxValue;
                }

                return pathResult.TotalCost;
            }
            catch (Exception ex)
            {
                _logger.LogWarning(
                    ex,
                    "[任务执行] 计算候选任务路径代价异常: RobotCode={RobotCode}, TaskCode={TaskCode}",
                    robot.RobotCode,
                    task.TaskCode);
                return double.MaxValue;
            }
        }

        /// <summary>
        /// 解析机器人在图中的起始节点：
        /// 优先使用当前节点ID；若缺失则按当前位置坐标匹配最近节点。
        /// </summary>
        private static Guid? ResolveStartNodeId(Robot robot, PathGraph graph)
        {
            if (robot.CurrentNodeId.HasValue && graph.Nodes.ContainsKey(robot.CurrentNodeId.Value))
            {
                return robot.CurrentNodeId.Value;
            }

            var sourceX = robot.CurrentX ?? robot.MapNode?.X;
            var sourceY = robot.CurrentY ?? robot.MapNode?.Y;
            if (!sourceX.HasValue || !sourceY.HasValue || graph.Nodes.Count == 0)
            {
                return null;
            }

            Guid? nearestNodeId = null;
            var nearestDistanceSquared = double.MaxValue;

            foreach (var node in graph.Nodes.Values)
            {
                if (!node.Active)
                {
                    continue;
                }

                var dx = sourceX.Value - node.X;
                var dy = sourceY.Value - node.Y;
                var distanceSquared = dx * dx + dy * dy;
                if (distanceSquared < nearestDistanceSquared)
                {
                    nearestDistanceSquared = distanceSquared;
                    nearestNodeId = node.NodeId;
                }
            }

            return nearestNodeId;
        }

        /// <summary>
        /// 候选任务评分上下文。
        /// 封装单个候选任务的排序比较信息。
        /// </summary>
        private sealed class TaskCandidate
        {
            public TaskCandidate(RobotTask task, double pathCost)
            {
                Task = task;
                PathCost = pathCost;
                CreatedAtMilliseconds = task.CreatedAt.Ticks / TimeSpan.TicksPerMillisecond;
            }

            public RobotTask Task { get; }
            public double PathCost { get; }
            public long CreatedAtMilliseconds { get; }
        }

        /// <summary>
        /// 仅按路径代价、创建时间与 TaskId 做稳定排序。
        /// </summary>
        private sealed class TaskCandidateComparer : IComparer<TaskCandidate>
        {
            public static readonly TaskCandidateComparer Instance = new();

            public int Compare(TaskCandidate? x, TaskCandidate? y)
            {
                if (ReferenceEquals(x, y)) return 0;
                if (x is null) return 1;
                if (y is null) return -1;

                var pathCostCompare = x.PathCost.CompareTo(y.PathCost);
                if (pathCostCompare != 0)
                {
                    return pathCostCompare;
                }

                var createdAtCompare = x.CreatedAtMilliseconds.CompareTo(y.CreatedAtMilliseconds);
                if (createdAtCompare != 0)
                {
                    return createdAtCompare;
                }

                return x.Task.TaskId.CompareTo(y.Task.TaskId);
            }
        }

        /// <summary>
        /// 检查机器人是否有执行中的任务
        /// @author zzy
        /// </summary>
        private async Task<bool> HasInProgressTaskAsync(
            Guid robotId,
            Guid taskId,
            IRobotTaskRepository taskRepo,
            CancellationToken cancellationToken)
        {
            var robotTasks = await taskRepo.GetByRobotIdAsync(robotId, cancellationToken);
            return robotTasks.Any(t =>
                t.TaskId != taskId &&
                t.SubTasks.Any(st => st.Status == TaskStatus.InProgress));
        }

        /// <summary>
        /// 根据协议类型执行任务
        /// @author zzy
        /// 重构为使用协议工厂模式
        /// </summary>
        private async Task<bool> ExecuteTaskByProtocolAsync(
            RobotTask task,
            Robot robot,
            IRobotTaskRepository taskRepo,
            IServiceScope scope,
            CancellationToken cancellationToken)
        {
            try
            {
                // 使用同一scope的taskRepo获取带详情的任务，避免跨scope实体跟踪冲突
                var taskWithDetails = await taskRepo.GetByIdWithDetailsAsync(task.TaskId, cancellationToken);
                if (taskWithDetails == null)
                {
                    _logger.LogWarning("[任务执行] 任务 {TaskCode} 详情获取失败", task.TaskCode);
                    return false;
                }
                if (taskWithDetails.SubTasks.Count <= 0 && robot.ProtocolType == ProtocolType.VDA)
                {
                    _logger.LogWarning("[任务执行] 任务 {TaskCode} 不存在子任务，执行失败", task.TaskCode);
                    return false;
                }

                // 获取协议服务工厂
                var protocolServiceFactory = scope.ServiceProvider.GetRequiredService<IProtocolServiceFactory>();
                var protocolService = protocolServiceFactory.GetService(robot);

                // 执行任务
                var res = await protocolService.PrepareSendOrderAsync(robot, taskWithDetails, cancellationToken);

                if (res.Success)
                {
                    taskWithDetails.StartExecution();
                    await taskRepo.UpdateAsync(taskWithDetails, cancellationToken);
                    await taskRepo.SaveChangesAsync(cancellationToken);
                    _logger.LogInformation("[任务执行] 任务 {TaskCode} 已下发给机器人 {RobotCode}，协议类型: {ProtocolType}",
                        taskWithDetails.TaskCode, robot.RobotCode, robot.ProtocolType);
                    return true;
                }
                else
                {
                    throw new Exception(res.Message);
                }
            }
            catch (Exception ex)
            {
                try
                {
                    var taskForErrorUpdate = await taskRepo.GetByIdAsync(task.TaskId, cancellationToken);
                    if (taskForErrorUpdate != null)
                    {
                        taskForErrorUpdate.ErrorInfo = ex.ToString();
                        taskForErrorUpdate.UpdatedAt = DateTime.Now;
                        await taskRepo.UpdateAsync(taskForErrorUpdate, cancellationToken);
                        await taskRepo.SaveChangesAsync(cancellationToken);
                    }
                }
                catch (Exception updateEx)
                {
                    _logger.LogError(updateEx, "[任务执行] 任务 {TaskCode} 错误信息更新失败", task.TaskCode);
                }

                _logger.LogError(ex, "[任务执行] 任务 {TaskCode} 执行失败", task.TaskCode);
                return false;
            }
        }
    }
}