zengzy / hhrcs_service · Files

using Microsoft.Extensions.DependencyInjection;
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
using Rcs.Application.Shared;
using Rcs.Domain.Entities;
using Rcs.Domain.Enums;
using Rcs.Domain.Repositories;
using TaskStatus = Rcs.Domain.Entities.TaskStatus;

namespace Rcs.Infrastructure.Services
{
    /// <summary>
    /// 后台任务调度服务 - 循环调度等待中的任务分配给空闲机器人
    /// @author zzy
    /// </summary>
    public class TaskDispatchBackgroundService : BackgroundService, ITaskDispatchService
    {
        private readonly ILogger<TaskDispatchBackgroundService> _logger;
        private readonly IServiceProvider _serviceProvider;
        private readonly IAgvPathService _agvPathService;
        private readonly TimeSpan _dispatchInterval = TimeSpan.FromSeconds(2);
        private const int MaxPendingTasksPerCycle = 10;
        private const double EtaPrioritySlackMeters = 5d;
        private const double EtaWeight = 0.55d;
        private const double DetourWeight = 0.20d;
        private const double LoadWeight = 0.20d;
        private const double TaskCountWeight = 0.05d;

        public TaskDispatchBackgroundService(
            ILogger<TaskDispatchBackgroundService> logger,
            IServiceProvider serviceProvider,
            IAgvPathService agvPathService)
        {
            _logger = logger;
            _serviceProvider = serviceProvider;
            _agvPathService = agvPathService;
        }

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

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

                await Task.Delay(_dispatchInterval, stoppingToken);
            }

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

        /// <summary>
        /// 执行一次任务调度
        /// @author zzy
        /// </summary>
        public async Task<TaskDispatchResult> DispatchAsync(CancellationToken cancellationToken = default)
        {
            using var scope = _serviceProvider.CreateScope();
            var taskRepo = scope.ServiceProvider.GetRequiredService<IRobotTaskRepository>();
            var robotRepo = scope.ServiceProvider.GetRequiredService<IRobotRepository>();
            var robotCache = scope.ServiceProvider.GetRequiredService<IRobotCacheService>();
            var templateRepo = scope.ServiceProvider.GetRequiredService<ITaskTemplateRepository>();
            var locationTypeRepo = scope.ServiceProvider.GetRequiredService<IStorageLocationTypeRepository>();
            var chargingPileRepo = scope.ServiceProvider.GetRequiredService<IChargingPileRepository>();

            // 1. 获取等待中的任务，按优先级升序（数值小优先级高）、创建时间升序排序，取前10个
            var pendingTasks = (await taskRepo.GetByStatusAsync(TaskStatus.Pending, cancellationToken)).OrderByDescending(t => t.Priority)
                .ThenBy(t => t.CreatedAt)
                .Take(MaxPendingTasksPerCycle)
                .ToList();

            if (!pendingTasks.Any())
            {
                return new TaskDispatchResult { Success = true, AssignedCount = 0, Message = "无待调度任务" };
            }

            int assignedCount = 0;

            foreach (var task in pendingTasks)
            {
                var taskWithDetails = await taskRepo.GetByIdWithDetailsAsync(task.TaskId, cancellationToken);
                if (taskWithDetails == null)
                    continue;

                // 获取任务起点所在地图的空闲机器人，筛选起点和终点库位类型都支持的机器人
                var robotDispatchInfo = await FindIdleRobotForTaskAsync(taskWithDetails, taskRepo, robotCache, robotRepo, locationTypeRepo, chargingPileRepo, cancellationToken);
                if (!robotDispatchInfo.HasValue) continue;

                var (idleRobot, availableCacheLocation) = robotDispatchInfo.Value;

                // 获取机器人对应的任务模板（包含步骤）
                var template = await GetTemplateForRobotAsync(idleRobot, templateRepo, cancellationToken);

                // 分配任务
                task.RobotId = idleRobot.RobotId;
                task.TaskTemplateId = template?.TemplateId;
                task.ShelfCode = availableCacheLocation.LocationCode;
                task.Status = TaskStatus.Assigned;
                task.UpdatedAt = DateTime.Now;
                availableCacheLocation.ContainerId = task.ContainerID;
                availableCacheLocation.UpdatedAt = DateTime.Now;

                await taskRepo.UpdateAsync(task, cancellationToken);
                await taskRepo.SaveChangesAsync(cancellationToken);

                // 根据模板创建子任务
                if (template != null && template.TaskSteps.Any())
                {
                    await CreateSubTasksFromTemplateAsync(taskWithDetails, idleRobot, template, cancellationToken);
                }

                _logger.LogInformation("[任务调度] 任务 {TaskCode} 已分配给机器人 {RobotCode}，模板: {TemplateCode}",
                    task.TaskCode, idleRobot.RobotCode, template?.TemplateCode ?? "无");

                assignedCount++;
            }

            return new TaskDispatchResult
            {
                Success = true,
                AssignedCount = assignedCount,
                Message = $"本次调度完成，已分配 {assignedCount} 个任务"
            };
        }

        /// <summary>
        /// 根据任务起点所在地图和库位类型查找空闲机器人
        /// 筛选起点和终点库位类型都支持的机器人
        /// @author zzy
        /// </summary>
        private async Task<(Robot Robot, RobotCacheLocation CacheLocation)?> FindIdleRobotForTaskAsync(
            RobotTask taskWithDetails,
            IRobotTaskRepository taskRepo,
            IRobotCacheService robotCache,
            IRobotRepository robotRepo,
            IStorageLocationTypeRepository locationTypeRepo,
            IChargingPileRepository chargingPileRepo,
            CancellationToken cancellationToken)
        {
            if (taskWithDetails?.BeginLocation?.MapNode == null)
            {
                _logger.LogWarning("[任务调度] 任务 {TaskCode} 无起点库位信息", taskWithDetails.TaskCode);
                return null;
            }

            if (taskWithDetails?.EndLocation?.MapNode == null)
            {
                _logger.LogWarning("[任务调度] 任务 {TaskCode} 无终点库位信息", taskWithDetails.TaskCode);
                return null;
            }

            var mapId = taskWithDetails.BeginLocation?.MapNode.MapId;

            // 获取起点和终点的库位类型
            var beginLocationTypeId = taskWithDetails.BeginLocation?.MapNode.StorageLocationTypeId;
            var endLocationTypeId = taskWithDetails.EndLocation?.MapNode.StorageLocationTypeId;

            if (!beginLocationTypeId.HasValue || !endLocationTypeId.HasValue)
            {
                _logger.LogWarning("[任务调度] 任务 {TaskCode} 库位类型信息不完整", taskWithDetails.TaskCode);
                return null;
            }

            // 获取库位类型详情
            var beginLocationType = await locationTypeRepo.GetByIdAsync(beginLocationTypeId.Value, cancellationToken);
            var endLocationType = await locationTypeRepo.GetByIdAsync(endLocationTypeId.Value, cancellationToken);

            if (beginLocationType == null || endLocationType == null)
            {
                _logger.LogWarning("[任务调度] 任务 {TaskCode} 库位类型不存在", taskWithDetails.TaskCode);
                return null;
            }

            // 获取该地图上所有在线的机器人
            var robotCacheData = await robotCache.GetAllActiveRobotCacheAsync();
            var idleRobots = robotCacheData.Select(r => new Robot
            {
                RobotId = Guid.Parse(r.Basic.RobotId),
                RobotCode = r.Basic.RobotCode,
                RobotName = r.Basic.RobotName,
                RobotVersion = r.Basic.RobotVersion,
                ProtocolName = r.Basic.ProtocolName,
                ProtocolVersion = r.Basic.ProtocolVersion,
                ProtocolType = (ProtocolType)r.Basic.ProtocolType,
                RobotManufacturer = r.Basic.RobotManufacturer,
                RobotSerialNumber = r.Basic.RobotSerialNumber,
                RobotModel = r.Basic.RobotModel,
                RobotType = (RobotType)r.Basic.RobotType,
                IpAddress = r.Basic.IpAddress,
                CoordinateScale = r.Basic.CoordinateScale,
                Active = r.Basic.Active,
                Status = r.Status?.Status ?? RobotStatus.Idle,
                Online = r.Status?.Online ?? OnlineStatus.Offline,
                BatteryLevel = r.Status?.BatteryLevel,
                Driving = r.Status?.Driving ?? false,
                Paused = r.Status?.Paused ?? false,
                Charging = r.Status?.Charging ?? false,
                OperatingMode = r.Status?.OperatingMode ?? OperatingMode.Automatic,
                CurrentMapCodeId = r.Location?.MapId,
                CurrentNodeId = r.Location?.NodeId,
                CurrentX = r.Location?.X,
                CurrentY = r.Location?.Y,
                CurrentTheta = r.Location?.Theta
            }).ToList();

            // 处理正在充电桩充电的机器人：
            // - 电量 > AutoStartThreshold：保留但排到列表末尾，低优先级参与调度
            // - 电量 < AutoStartThreshold：从候选列表移除，不参与调度
            // @author zzy
            var activePiles = (await chargingPileRepo.FindAsync(
                    p => p.IsActive && p.CurrentChargingRobotId != null, cancellationToken))
                .ToList();

            var highBatteryChargingRobotIds = new HashSet<Guid>();
            foreach (var pile in activePiles)
            {
                var robotId = pile.CurrentChargingRobotId!.Value;
                var chargingRobot = idleRobots.FirstOrDefault(r => r.RobotId == robotId);
                if (chargingRobot == null) continue;

                var batteryLevel = chargingRobot.BatteryLevel ?? 0m;
                if (batteryLevel > pile.AutoStartThreshold)
                {
                    // 电量充足，保留但标记为低优先级
                    highBatteryChargingRobotIds.Add(robotId);
                } else
                {
                    // 电量不足且仍在充电，移除不参与调度
                    idleRobots.RemoveAll(r => r.RobotId == robotId);
                }
            }

            // 将高电量充电中的机器人排到列表末尾
            if (highBatteryChargingRobotIds.Count > 0)
            {
                var highBatteryRobots = idleRobots.Where(r => highBatteryChargingRobotIds.Contains(r.RobotId)).ToList();
                idleRobots.RemoveAll(r => highBatteryChargingRobotIds.Contains(r.RobotId));
                idleRobots.AddRange(highBatteryRobots);
            }

            // 先进行同步条件筛选
            var candidateRobots = idleRobots
                .Where(r => r.CurrentMapCodeId == mapId
                            && r.Online == OnlineStatus.Online
                            && r.Active == true
                            && r.Status != RobotStatus.Error
                            && r.Paused == false
                            && ((r.CurrentNodeId != null && !string.IsNullOrWhiteSpace(r.CurrentNodeId.ToString()) && !r.Driving && r.ProtocolType == ProtocolType.VDA) || r.ProtocolType == ProtocolType.Custom)
                            && r.RobotModel != null
                            && beginLocationType.RobotModels.Contains(r.RobotModel)
                            && endLocationType.RobotModels.Contains(r.RobotModel))
                .ToList();

            if (!candidateRobots.Any())
            {
                return null;
            }

            // 若主任务已指定机器人，根据是否同时指定缓存货架编号走不同分支
            // 1. 仅指定机器人（无ShelfCode）：校验机器人可用性，由低到高自动查找空闲货位
            // 2. 同时指定机器人和缓存货架编号：校验对应缓存库位是否为空
            // @author zzy
            if (taskWithDetails.RobotId.HasValue)
            {
                var specifiedRobotId = taskWithDetails.RobotId.Value;
                var specifiedLiveRobot = candidateRobots.FirstOrDefault(r => r.RobotId == specifiedRobotId);
                if (specifiedLiveRobot == null)
                {
                    _logger.LogInformation(
                        "[任务调度] 任务 {TaskCode} 指定机器人 {RobotId} 当前不可调度，跳过本轮",
                        taskWithDetails.TaskCode,
                        specifiedRobotId);
                    return null;
                }

                var specifiedRobotWithCacheLocations = await robotRepo.GetByIdFullDataAsync(specifiedRobotId, cancellationToken);
                if (specifiedRobotWithCacheLocations == null)
                {
                    _logger.LogWarning(
                        "[任务调度] 任务 {TaskCode} 指定机器人 {RobotId} 不存在，跳过本轮",
                        taskWithDetails.TaskCode,
                        specifiedRobotId);
                    return null;
                }

                // 场景2：同时指定了机器人和缓存货架编号，校验对应缓存库位是否为空
                if (!string.IsNullOrWhiteSpace(taskWithDetails.ShelfCode))
                {
                    var specifiedShelfCode = taskWithDetails.ShelfCode.Trim();
                    var specifiedCacheLocation = specifiedRobotWithCacheLocations.CacheLocations
                        .FirstOrDefault(c => c.LocationCode == specifiedShelfCode);
                    if (specifiedCacheLocation == null)
                    {
                        _logger.LogWarning(
                            "[任务调度] 任务 {TaskCode} 指定缓存位 {ShelfCode} 不属于机器人 {RobotCode}，跳过本轮",
                            taskWithDetails.TaskCode,
                            specifiedShelfCode,
                            specifiedRobotWithCacheLocations.RobotCode);
                        return null;
                    }

                    if (!string.IsNullOrWhiteSpace(specifiedCacheLocation.ContainerId))
                    {
                        _logger.LogInformation(
                            "[任务调度] 任务 {TaskCode} 指定缓存位 {ShelfCode} 非空，等待释放",
                            taskWithDetails.TaskCode,
                            specifiedShelfCode);
                        return null;
                    }

                    return (specifiedRobotWithCacheLocations, specifiedCacheLocation);
                }

                // 场景1：仅指定了机器人（无ShelfCode），由低到高自动查找空闲货位
                var autoAvailableCacheLocation = specifiedRobotWithCacheLocations.CacheLocations
                    .Where(c => string.IsNullOrWhiteSpace(c.ContainerId))
                    .OrderBy(c => c.Level)
                    .ThenBy(c => c.Column)
                    .ThenBy(c => c.Row)
                    .ThenBy(c => c.LocationCode)
                    .FirstOrDefault();

                if (autoAvailableCacheLocation == null)
                {
                    _logger.LogInformation(
                        "[任务调度] 任务 {TaskCode} 指定机器人 {RobotCode} 无空闲缓存货位，跳过本轮",
                        taskWithDetails.TaskCode,
                        specifiedRobotWithCacheLocations.RobotCode);
                    return null;
                }

                return (specifiedRobotWithCacheLocations, autoAvailableCacheLocation);
            }

            var dispatchTargetPoint = GetDispatchTargetPoint(taskWithDetails);
            if (dispatchTargetPoint == null)
            {
                _logger.LogWarning("[任务调度] 任务 {TaskCode} 缺少可用于调度评分的目标点信息", taskWithDetails.TaskCode);
                return null;
            }

            var dispatchCandidates = new List<DispatchRobotCandidate>();
            foreach (var liveRobot in candidateRobots)
            {
                var robotWithCacheLocations = await robotRepo.GetByIdFullDataAsync(liveRobot.RobotId, cancellationToken);
                if (robotWithCacheLocations == null)
                {
                    continue;
                }

                // 多缓存储位场景：按“由低到高、由左到右、由前到后”分配空缓存位
                var availableCacheLocation = robotWithCacheLocations.CacheLocations
                    .Where(c => string.IsNullOrWhiteSpace(c.ContainerId))
                    .OrderBy(c => c.Level)
                    .ThenBy(c => c.Column)
                    .ThenBy(c => c.Row)
                    .ThenBy(c => c.LocationCode)
                    .FirstOrDefault();

                if (availableCacheLocation != null)
                {
                    var currentX = liveRobot.CurrentX ?? robotWithCacheLocations.CurrentX ?? robotWithCacheLocations.MapNode?.X;
                    var currentY = liveRobot.CurrentY ?? robotWithCacheLocations.CurrentY ?? robotWithCacheLocations.MapNode?.Y;

                    dispatchCandidates.Add(new DispatchRobotCandidate(
                        robotWithCacheLocations,
                        availableCacheLocation,
                        currentX,
                        currentY));
                }
            }

            if (!dispatchCandidates.Any())
            {
                return null;
            }

            var candidateRobotIds = dispatchCandidates
                .Select(c => c.Robot.RobotId)
                .ToHashSet();

            var assignedTasks = (await taskRepo.GetByStatusAsync(TaskStatus.Assigned, cancellationToken))
                .Where(t => t.RobotId.HasValue && candidateRobotIds.Contains(t.RobotId.Value));

            var inProgressTasks = (await taskRepo.GetByStatusAsync(TaskStatus.InProgress, cancellationToken))
                .Where(t => t.RobotId.HasValue && candidateRobotIds.Contains(t.RobotId.Value));

            var activeTaskIds = assignedTasks
                .Concat(inProgressTasks)
                .Select(t => t.TaskId)
                .Distinct()
                .ToList();

            var activeTasksWithDetails = new List<RobotTask>(activeTaskIds.Count);
            foreach (var activeTaskId in activeTaskIds)
            {
                var activeTaskWithDetails = await taskRepo.GetByIdWithDetailsAsync(activeTaskId, cancellationToken);
                if (activeTaskWithDetails?.RobotId.HasValue == true
                    && candidateRobotIds.Contains(activeTaskWithDetails.RobotId.Value))
                {
                    activeTasksWithDetails.Add(activeTaskWithDetails);
                }
            }

            var activeTasksByRobot = activeTasksWithDetails
                .Where(t => t.RobotId.HasValue)
                .GroupBy(t => t.RobotId!.Value)
                .ToDictionary(g => g.Key, g => (IReadOnlyCollection<RobotTask>)g.ToList());

            var mapGraphCache = new Dictionary<Guid, PathGraph?>();
            var scoreCards = new List<DispatchRobotScore>(dispatchCandidates.Count);
            foreach (var candidate in dispatchCandidates)
            {
                activeTasksByRobot.TryGetValue(candidate.Robot.RobotId, out var robotTasks);
                scoreCards.Add(await BuildDispatchScoreAsync(
                    candidate,
                    robotTasks ?? Array.Empty<RobotTask>(),
                    dispatchTargetPoint.Value,
                    taskWithDetails.Priority,
                    mapGraphCache,
                    cancellationToken));
            }

            var selectedScore = SelectBestScore(scoreCards);
            if (selectedScore == null)
            {
                return null;
            }

            _logger.LogInformation(
                "[任务调度] 任务 {TaskCode} 分配评分结果: 机器人={RobotCode}, ETA={EtaDistance:F2}, 增量={DetourDistance:F2}, 负载={LoadDistance:F2}, 任务数={TaskCount}, 综合分={CompositeScore:F4}",
                taskWithDetails.TaskCode,
                selectedScore.Robot.RobotCode,
                selectedScore.EtaDistance,
                selectedScore.DetourDistance,
                selectedScore.LoadDistance,
                selectedScore.ActiveTaskCount,
                selectedScore.CompositeScore);

            return (selectedScore.Robot, selectedScore.CacheLocation);
        }

        private static DispatchPoint? GetDispatchTargetPoint(RobotTask task)
        {
            var nextSubTask = task.GetNextExecutableSubTask();
            var nextTargetNode = nextSubTask?.EndNode ?? task.BeginLocation?.MapNode;
            if (nextTargetNode != null)
            {
                return new DispatchPoint(
                    nextTargetNode.MapId,
                    nextTargetNode.NodeId,
                    nextTargetNode.X,
                    nextTargetNode.Y,
                    nextTargetNode.Theta);
            }

            return null;
        }

        private async Task<DispatchRobotScore> BuildDispatchScoreAsync(
            DispatchRobotCandidate candidate,
            IReadOnlyCollection<RobotTask> activeTasks,
            DispatchPoint dispatchTargetPoint,
            int taskPriority,
            IDictionary<Guid, PathGraph?> mapGraphCache,
            CancellationToken cancellationToken)
        {
            var robotStartPoint = await ResolveRobotStartPointAsync(
                candidate,
                dispatchTargetPoint.MapId,
                mapGraphCache);

            if (!robotStartPoint.HasValue)
            {
                return new DispatchRobotScore(
                    candidate,
                    double.MaxValue,
                    double.MaxValue,
                    double.MaxValue,
                    activeTasks.Count,
                    double.MaxValue);
            }

            var pendingTaskPoints = activeTasks
                .Select(GetDispatchTargetPoint)
                .Where(point => point.HasValue)
                .Select(point => point!.Value)
                .ToList();

            var pathCostCache = new Dictionary<PathCostCacheKey, double>();

            var executionChain = await BuildNearestNeighborChainAsync(
                candidate.Robot,
                robotStartPoint.Value,
                pendingTaskPoints,
                taskPriority,
                mapGraphCache,
                pathCostCache,
                cancellationToken);

            var loadDistance = await CalculatePathDistanceAsync(
                candidate.Robot,
                robotStartPoint.Value,
                executionChain,
                taskPriority,
                mapGraphCache,
                pathCostCache,
                cancellationToken);

            var insertion = await EvaluateBestInsertionAsync(
                candidate.Robot,
                robotStartPoint.Value,
                executionChain,
                dispatchTargetPoint,
                taskPriority,
                mapGraphCache,
                pathCostCache,
                cancellationToken);

            return new DispatchRobotScore(
                candidate,
                insertion.EtaDistance,
                insertion.DetourDistance,
                loadDistance,
                pendingTaskPoints.Count,
                0d);
        }

        private async Task<DispatchInsertionResult> EvaluateBestInsertionAsync(
            Robot robot,
            DispatchPoint start,
            IReadOnlyList<DispatchPoint> chain,
            DispatchPoint target,
            int taskPriority,
            IDictionary<Guid, PathGraph?> mapGraphCache,
            IDictionary<PathCostCacheKey, double> pathCostCache,
            CancellationToken cancellationToken)
        {
            if (chain.Count == 0)
            {
                var directDistance = await CalculateTravelCostAsync(
                    robot,
                    start,
                    target,
                    taskPriority,
                    mapGraphCache,
                    pathCostCache,
                    cancellationToken);
                return new DispatchInsertionResult(directDistance, directDistance);
            }

            var arrivalsAtChainPoint = new double[chain.Count];
            arrivalsAtChainPoint[0] = await CalculateTravelCostAsync(
                robot,
                start,
                chain[0],
                taskPriority,
                mapGraphCache,
                pathCostCache,
                cancellationToken);
            for (int i = 1; i < chain.Count; i++)
            {
                arrivalsAtChainPoint[i] = arrivalsAtChainPoint[i - 1]
                                          + await CalculateTravelCostAsync(
                                              robot,
                                              chain[i - 1],
                                              chain[i],
                                              taskPriority,
                                              mapGraphCache,
                                              pathCostCache,
                                              cancellationToken);
            }

            double bestEta = double.MaxValue;
            double bestDetour = double.MaxValue;

            for (int insertIndex = 0; insertIndex <= chain.Count; insertIndex++)
            {
                var previousPoint = insertIndex == 0 ? start : chain[insertIndex - 1];
                var previousArrival = insertIndex == 0 ? 0d : arrivalsAtChainPoint[insertIndex - 1];
                var prevToTargetCost = await CalculateTravelCostAsync(
                    robot,
                    previousPoint,
                    target,
                    taskPriority,
                    mapGraphCache,
                    pathCostCache,
                    cancellationToken);
                var etaDistance = previousArrival + prevToTargetCost;

                double detourDistance;
                if (insertIndex == chain.Count)
                {
                    detourDistance = prevToTargetCost;
                }
                else
                {
                    var nextPoint = chain[insertIndex];
                    var targetToNextCost = await CalculateTravelCostAsync(
                        robot,
                        target,
                        nextPoint,
                        taskPriority,
                        mapGraphCache,
                        pathCostCache,
                        cancellationToken);
                    var prevToNextCost = await CalculateTravelCostAsync(
                        robot,
                        previousPoint,
                        nextPoint,
                        taskPriority,
                        mapGraphCache,
                        pathCostCache,
                        cancellationToken);
                    detourDistance = prevToTargetCost + targetToNextCost - prevToNextCost;
                }

                if (etaDistance < bestEta || (Math.Abs(etaDistance - bestEta) < 0.0001d && detourDistance < bestDetour))
                {
                    bestEta = etaDistance;
                    bestDetour = detourDistance;
                }
            }

            return new DispatchInsertionResult(bestEta, bestDetour);
        }

        private async Task<List<DispatchPoint>> BuildNearestNeighborChainAsync(
            Robot robot,
            DispatchPoint start,
            IReadOnlyCollection<DispatchPoint> points,
            int taskPriority,
            IDictionary<Guid, PathGraph?> mapGraphCache,
            IDictionary<PathCostCacheKey, double> pathCostCache,
            CancellationToken cancellationToken)
        {
            var remaining = points.ToList();
            var chain = new List<DispatchPoint>(remaining.Count);
            var current = start;

            while (remaining.Count > 0)
            {
                int nearestIndex = 0;
                double nearestDistance = double.MaxValue;

                for (int i = 0; i < remaining.Count; i++)
                {
                    var distance = await CalculateTravelCostAsync(
                        robot,
                        current,
                        remaining[i],
                        taskPriority,
                        mapGraphCache,
                        pathCostCache,
                        cancellationToken);
                    if (distance < nearestDistance)
                    {
                        nearestDistance = distance;
                        nearestIndex = i;
                    }
                }

                current = remaining[nearestIndex];
                chain.Add(current);
                remaining.RemoveAt(nearestIndex);
            }

            return chain;
        }

        private async Task<double> CalculatePathDistanceAsync(
            Robot robot,
            DispatchPoint start,
            IReadOnlyList<DispatchPoint> chain,
            int taskPriority,
            IDictionary<Guid, PathGraph?> mapGraphCache,
            IDictionary<PathCostCacheKey, double> pathCostCache,
            CancellationToken cancellationToken)
        {
            if (chain.Count == 0)
            {
                return 0d;
            }

            var totalDistance = await CalculateTravelCostAsync(
                robot,
                start,
                chain[0],
                taskPriority,
                mapGraphCache,
                pathCostCache,
                cancellationToken);
            for (int i = 1; i < chain.Count; i++)
            {
                totalDistance += await CalculateTravelCostAsync(
                    robot,
                    chain[i - 1],
                    chain[i],
                    taskPriority,
                    mapGraphCache,
                    pathCostCache,
                    cancellationToken);
            }

            return totalDistance;
        }

        private static DispatchRobotScore? SelectBestScore(IReadOnlyCollection<DispatchRobotScore> scoreCards)
        {
            if (!scoreCards.Any())
            {
                return null;
            }

            var bestEta = scoreCards.Min(s => s.EtaDistance);
            var etaPreferred = scoreCards
                .Where(s => s.EtaDistance <= bestEta + EtaPrioritySlackMeters)
                .ToList();

            var etaMin = etaPreferred.Min(s => s.EtaDistance);
            var etaMax = etaPreferred.Max(s => s.EtaDistance);
            var detourMin = etaPreferred.Min(s => s.DetourDistance);
            var detourMax = etaPreferred.Max(s => s.DetourDistance);
            var loadMin = etaPreferred.Min(s => s.LoadDistance);
            var loadMax = etaPreferred.Max(s => s.LoadDistance);
            var taskCountMin = etaPreferred.Min(s => s.ActiveTaskCount);
            var taskCountMax = etaPreferred.Max(s => s.ActiveTaskCount);

            var rescored = etaPreferred
                .Select(s =>
                {
                    var score = EtaWeight * Normalize(s.EtaDistance, etaMin, etaMax)
                                + DetourWeight * Normalize(s.DetourDistance, detourMin, detourMax)
                                + LoadWeight * Normalize(s.LoadDistance, loadMin, loadMax)
                                + TaskCountWeight * Normalize(s.ActiveTaskCount, taskCountMin, taskCountMax);

                    return s with { CompositeScore = score };
                })
                .ToList();

            return rescored
                .OrderBy(s => s.CompositeScore)
                .ThenBy(s => s.EtaDistance)
                .ThenBy(s => s.LoadDistance)
                .ThenBy(s => s.ActiveTaskCount)
                .ThenBy(s => s.Robot.RobotCode)
                .FirstOrDefault();
        }

        private async Task<PathGraph?> GetPathGraphAsync(
            Guid mapId,
            IDictionary<Guid, PathGraph?> mapGraphCache)
        {
            if (mapGraphCache.TryGetValue(mapId, out var cachedGraph))
            {
                return cachedGraph;
            }

            var graph = await _agvPathService.GetOrBuildGraphAsync(mapId);
            mapGraphCache[mapId] = graph;
            return graph;
        }

        private async Task<DispatchPoint?> ResolveRobotStartPointAsync(
            DispatchRobotCandidate candidate,
            Guid mapId,
            IDictionary<Guid, PathGraph?> mapGraphCache)
        {
            var sourceX = candidate.CurrentX ?? candidate.Robot.CurrentX ?? candidate.Robot.MapNode?.X;
            var sourceY = candidate.CurrentY ?? candidate.Robot.CurrentY ?? candidate.Robot.MapNode?.Y;
            var sourceTheta = candidate.Robot.CurrentTheta ?? candidate.Robot.MapNode?.Theta;
            var startNodeId = candidate.Robot.CurrentNodeId;

            PathGraph? graph = null;
            if (startNodeId.HasValue || !sourceX.HasValue || !sourceY.HasValue)
            {
                graph = await GetPathGraphAsync(mapId, mapGraphCache);
            }

            if (startNodeId.HasValue && graph?.Nodes.TryGetValue(startNodeId.Value, out var currentNode) == true)
            {
                sourceX ??= currentNode.X;
                sourceY ??= currentNode.Y;
                sourceTheta ??= currentNode.Theta;
            }

            if (!startNodeId.HasValue && sourceX.HasValue && sourceY.HasValue && graph != null)
            {
                Guid? nearestNodeId = null;
                double? nearestNodeTheta = 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;
                        nearestNodeTheta = node.Theta;
                    }
                }

                startNodeId = nearestNodeId;
                sourceTheta ??= nearestNodeTheta;
            }

            if (!sourceX.HasValue || !sourceY.HasValue)
            {
                return null;
            }

            return new DispatchPoint(mapId, startNodeId, sourceX.Value, sourceY.Value, sourceTheta);
        }

        private async Task<double> CalculateTravelCostAsync(
            Robot robot,
            DispatchPoint source,
            DispatchPoint target,
            int taskPriority,
            IDictionary<Guid, PathGraph?> mapGraphCache,
            IDictionary<PathCostCacheKey, double> pathCostCache,
            CancellationToken cancellationToken)
        {
            var fallbackDistance = CalculateEuclideanDistance(source, target);

            if (source.MapId != target.MapId
                || !source.NodeId.HasValue
                || !target.NodeId.HasValue)
            {
                return fallbackDistance;
            }

            var cacheKey = new PathCostCacheKey(
                robot.RobotId,
                source.NodeId.Value,
                target.NodeId.Value,
                RoundForPathCostCache(source.Theta),
                RoundForPathCostCache(target.Theta));
            if (pathCostCache.TryGetValue(cacheKey, out var cachedCost))
            {
                return cachedCost;
            }

            var graph = await GetPathGraphAsync(source.MapId, mapGraphCache);
            if (graph == null
                || !graph.Nodes.ContainsKey(source.NodeId.Value)
                || !graph.Nodes.ContainsKey(target.NodeId.Value))
            {
                pathCostCache[cacheKey] = fallbackDistance;
                return fallbackDistance;
            }

            var request = new PathRequest
            {
                RobotId = robot.RobotId,
                MapId = source.MapId,
                StartNodeId = source.NodeId.Value,
                EndNodeId = target.NodeId.Value,
                CurrentTheta = source.Theta ?? robot.CurrentTheta ?? 0d,
                IsLoaded = false,
                Priority = taskPriority,
                BatteryLevel = robot.BatteryLevel ?? 100,
                MovementType = robot.MovementType,
                ForkRadOffsets = robot.ForkRadOffset,
                RequiredEndRad = target.Theta
            };

            try
            {
                var pathResult = await _agvPathService.CalculatePathAsync(request, cancellationToken);
                if (pathResult.Success
                    && !double.IsNaN(pathResult.TotalCost)
                    && !double.IsInfinity(pathResult.TotalCost))
                {
                    pathCostCache[cacheKey] = pathResult.TotalCost;
                    return pathResult.TotalCost;
                }
            }
            catch (Exception ex)
            {
                _logger.LogDebug(
                    ex,
                    "[任务调度] 路径评分异常，回退直线距离: RobotCode={RobotCode}, StartNodeId={StartNodeId}, EndNodeId={EndNodeId}",
                    robot.RobotCode,
                    source.NodeId,
                    target.NodeId);
            }

            pathCostCache[cacheKey] = fallbackDistance;
            return fallbackDistance;
        }

        /// <summary>
        /// 计算两点直线距离。
        /// </summary>
        private static double CalculateEuclideanDistance(DispatchPoint source, DispatchPoint target)
        {
            var dx = source.X - target.X;
            var dy = source.Y - target.Y;
            return Math.Sqrt(dx * dx + dy * dy);
        }

        private static double RoundForPathCostCache(double? value)
        {
            return value.HasValue ? Math.Round(value.Value, 4) : double.NaN;
        }

        private static double Normalize(double value, double min, double max)
        {
            if (double.IsNaN(value) || value == double.MaxValue)
            {
                return 1d;
            }

            var range = max - min;
            if (Math.Abs(range) < 0.0001d)
            {
                return 0d;
            }

            return (value - min) / range;
        }

        private readonly record struct DispatchPoint(
            Guid MapId,
            Guid? NodeId,
            double X,
            double Y,
            double? Theta);

        private readonly record struct DispatchInsertionResult(double EtaDistance, double DetourDistance);

        private readonly record struct PathCostCacheKey(
            Guid RobotId,
            Guid StartNodeId,
            Guid EndNodeId,
            double StartTheta,
            double EndTheta);

        private sealed record DispatchRobotCandidate(
            Robot Robot,
            RobotCacheLocation CacheLocation,
            double? CurrentX,
            double? CurrentY);

        private sealed record DispatchRobotScore(
            DispatchRobotCandidate Candidate,
            double EtaDistance,
            double DetourDistance,
            double LoadDistance,
            int ActiveTaskCount,
            double CompositeScore)
        {
            public Robot Robot => Candidate.Robot;
            public RobotCacheLocation CacheLocation => Candidate.CacheLocation;
        }

        /// <summary>
        /// 获取机器人对应的任务模板（包含步骤和属性）
        /// 优先获取默认模板，默认模板优先
        /// @author zzy
        /// </summary>
        private async Task<TaskTemplate?> GetTemplateForRobotAsync(
            Robot robot,
            ITaskTemplateRepository templateRepo,
            CancellationToken cancellationToken)
        {
            // 优先获取该机器人类型和制造商的默认模板
            var template = await templateRepo.GetDefaultTemplateAsync(
                robot.RobotType,
                robot.RobotManufacturer,
                cancellationToken: cancellationToken);

            TaskTemplate? resultTemplate = null;
            if (template != null)
            {
                // 获取包含完整详情的模板（步骤、属性、动作）
                resultTemplate = await templateRepo.GetWithFullDetailsAsync(template.TemplateId, cancellationToken);
            }

            if (resultTemplate != null) return resultTemplate;

            // 如果没有默认模板，获取该机器人类型的任意启用模板
            var templates = await templateRepo.GetByRobotTypeAsync(robot.RobotType, cancellationToken);
            var fallbackTemplate = templates.FirstOrDefault(t => t.IsEnabled);

            if (fallbackTemplate != null)
            {
                // 获取包含完整详情的模板（步骤、属性、动作）
                resultTemplate = await templateRepo.GetWithFullDetailsAsync(fallbackTemplate.TemplateId, cancellationToken);
            }

            return resultTemplate;
        }

        /// <summary>
        /// 根据模板中的步骤创建子任务
        /// 以模板中的order排序创建子任务
        /// 除了第一个子任务，后续的子任务的起点都是上一个子任务的终点
        /// 根据step中的Node属性类型(NodeValueType)来确定终点
        /// @author zzy
        /// </summary>
        private async Task CreateSubTasksFromTemplateAsync(
            RobotTask taskWithDetails,
            Robot robot,
            TaskTemplate template,
            CancellationToken cancellationToken)
        {
            using var scope = _serviceProvider.CreateScope();
            var subTaskRepo = scope.ServiceProvider.GetRequiredService<IRobotSubTaskRepository>();
            var taskRepo = scope.ServiceProvider.GetRequiredService<IRobotTaskRepository>();

            // 按order排序获取模板步骤
            var orderedSteps = template.TaskSteps
                .OrderBy(s => s.Order)
                .ToList();

            if (orderedSteps.Count == 0)
            {
                _logger.LogWarning("[任务调度] 模板 {TemplateCode} 无步骤配置", template.TemplateCode);
                return;
            }

            if (taskWithDetails?.BeginLocation?.MapNode == null
                || taskWithDetails?.EndLocation?.MapNode == null)
            {
                _logger.LogWarning("[任务调度] 任务 {TaskCode} 缺少起点或终点节点信息", taskWithDetails.TaskCode);
                return;
            }

            Guid beginNodeId = taskWithDetails.BeginLocation.MapNode.NodeId;
            Guid endNodeId = taskWithDetails.EndLocation.MapNode.NodeId;

            // 跟踪上一个子任务的终点：
            // 优先使用机器人当前位置节点，缺失时回退到任务起点，避免可空值取Value导致异常
            Guid previousEndNodeId = robot.CurrentNodeId ?? beginNodeId;
            if (!robot.CurrentNodeId.HasValue)
            {
                _logger.LogWarning(
                    "[任务调度] 任务 {TaskCode} 创建子任务时机器人 {RobotCode} 缺少当前节点，使用任务起点节点 {BeginNodeId} 作为首段起点",
                    taskWithDetails.TaskCode,
                    robot.RobotCode,
                    beginNodeId);
            }
            int sequence = 1;

            foreach (var step in orderedSteps)
            {
                var subTask = new RobotSubTask
                {
                    SubTaskId = Guid.NewGuid(),
                    TaskId = taskWithDetails.TaskId,
                    RobotId = robot.RobotId,
                    Status = TaskStatus.Pending,
                    CreatedAt = DateTime.Now,
                    Sequence = sequence
                };

                // 设置子任务起点：第一个为机器人当前节点，后续为上一个子任务的终点

                subTask.BeginNodeId = previousEndNodeId;
                

                // 根据步骤的Node属性确定终点
                var nodeProperty = step.Properties?.FirstOrDefault(p => p.PropertyType == StepPropertyType.Node);

                subTask.EndNodeId = nodeProperty?.NodeValue.HasValue == true
                    ? nodeProperty.NodeValue.Value switch
                    {
                        NodeValueType.Ts => beginNodeId,  // 任务起点
                        NodeValueType.Te => endNodeId,    // 任务终点
                        NodeValueType.Ws => endNodeId,    // 工位集合 - 暂时使用任务终点，后续可根据具体业务逻辑确定工位节点
                        _ => endNodeId                     // 默认使用任务终点
                    }
                    : endNodeId;  // 没有配置Node属性，默认使用任务终点

                await subTaskRepo.AddAsync(subTask, cancellationToken);

                previousEndNodeId = subTask.EndNodeId;
                sequence++;

                _logger.LogInformation("[任务调度] 创建子任务: 任务={TaskCode}, 子任务ID={SubTaskId}, 顺序={Sequence}, 起点={BeginNode}, 终点={EndNode}",
                    taskWithDetails.TaskCode, subTask.SubTaskId, subTask.Sequence, subTask.BeginNodeId, subTask.EndNodeId);
            }

            await subTaskRepo.SaveChangesAsync(cancellationToken);
        }
    }
}