zengzy / hhrcs_service · Files

using Microsoft.EntityFrameworkCore;
using Microsoft.Extensions.Logging;
using Rcs.Application.Services;
using Rcs.Application.Services.Protocol;
using Rcs.Domain.Entities;
using Rcs.Domain.Entities.DomainEvents.Robot;
using Rcs.Domain.Enums;
using Rcs.Domain.Repositories;
using TaskStatus = Rcs.Domain.Entities.TaskStatus;

namespace Rcs.Infrastructure.Services;
/// <summary>
/// 自动充电流程服务（精简版）。
/// </summary>
public class ChargingFlowService : IChargingFlowService
{
    private const string AutoChargeSource = "AutoCharge";

    private readonly ILogger<ChargingFlowService> _logger;
    private readonly IRobotRepository _robotRepository;
    private readonly IRobotTaskRepository _robotTaskRepository;
    private readonly IChargingPileRepository _chargingPileRepository;
    private readonly IActionConfigurationRepository _actionConfigurationRepository;
    private readonly IMapNodeRepository _mapNodeRepository;
    private readonly IProtocolServiceFactory _protocolServiceFactory;

    public ChargingFlowService(
        ILogger<ChargingFlowService> logger,
        IRobotRepository robotRepository,
        IRobotTaskRepository robotTaskRepository,
        IChargingPileRepository chargingPileRepository,
        IActionConfigurationRepository actionConfigurationRepository,
        IMapNodeRepository mapNodeRepository,
        IProtocolServiceFactory protocolServiceFactory)
    {
        _logger = logger;
        _robotRepository = robotRepository;
        _robotTaskRepository = robotTaskRepository;
        _chargingPileRepository = chargingPileRepository;
        _actionConfigurationRepository = actionConfigurationRepository;
        _mapNodeRepository = mapNodeRepository;
        _protocolServiceFactory = protocolServiceFactory;
    }
    /// <summary>
    /// 处理机器人状态变更事件。
    /// </summary>
    public async Task HandleRobotStatusChangedAsync(RobotStatusChangedDomainEvent domainEvent, CancellationToken cancellationToken = default)
    {
        var robot = await _robotRepository.GetByIdFullDataAsync(domainEvent.RobotId, cancellationToken);
        if (robot == null)
        {
            return;
        }

        await ProcessRobotAsync(robot, cancellationToken);
    }
    /// <summary>
    /// 周期巡检自动充电逻辑。
    /// </summary>
    public async Task ReconcileAsync(CancellationToken cancellationToken = default)
    {
        var robots = await _robotRepository.GetActiveRobotsAsync(cancellationToken);
        foreach (var robot in robots)
        {
            if (cancellationToken.IsCancellationRequested)
            {
                break;
            }

            await ProcessRobotAsync(robot, cancellationToken);
        }
    }
    /// <summary>
    /// 根据机器人状态分发充电处理分支。
    /// </summary>
    private async Task ProcessRobotAsync(Robot robot, CancellationToken cancellationToken)
    {
        if (robot.ProtocolType != ProtocolType.VDA || !robot.Active || robot.Online != OnlineStatus.Online)
        {
            return;
        }

        if (robot.Charging)
        {
            await HandleChargingRobotAsync(robot, cancellationToken);
            return;
        }

        await HandleNotChargingRobotAsync(robot, cancellationToken);
    }
    /// <summary>
    /// 处理机器人已在充电中的逻辑。
    /// </summary>
    private async Task HandleChargingRobotAsync(Robot robot, CancellationToken cancellationToken)
    {
        var pile = await GetCurrentChargingPileAsync(robot, cancellationToken);
        if (pile == null)
        {
            return;
        }

        if (!ShouldStopCharging(robot, pile))
        {
            return;
        }
        var protocol = _protocolServiceFactory.GetService(robot);
        var response = await protocol.StopChargingAsync(robot, cancellationToken);
        if (!response.Success)
        {
            _logger.LogWarning(
                "Stop charging action failed. Robot={RobotCode}, Pile={PileCode}, Message={Message}",
                robot.RobotCode,
                pile.PileCode,
                response.Message);
        }
    }
    /// <summary>
    /// 处理机器人未充电时的自动充电触发逻辑。
    /// </summary>
    private async Task HandleNotChargingRobotAsync(Robot robot, CancellationToken cancellationToken)
    {
        if (!robot.BatteryLevel.HasValue || robot.Driving || robot.Paused || robot.Status != RobotStatus.Idle)
        {
            return;
        }

        var candidatePile = await SelectChargingPileAsync(robot, cancellationToken);
        if(candidatePile == null)
        {
            return;
        }

        await CreateNavigationTaskAsync(robot, candidatePile, cancellationToken);
    }
    /// <summary>
    /// 根据当前位置获取机器人所在充电桩。
    /// </summary>
    private async Task<ChargingPile?> GetCurrentChargingPileAsync(Robot robot, CancellationToken cancellationToken)
    {
        if (!robot.CurrentNodeId.HasValue)
        {
            return null;
        }

        return await _chargingPileRepository.GetQueryable()
            .Where(p => p.IsActive && p.MapNodeId.HasValue && p.MapNodeId.Value == robot.CurrentNodeId.Value)
            .OrderBy(p => p.Priority)
            .ThenBy(p => p.PileCode)
            .FirstOrDefaultAsync(cancellationToken);
    }
    /// <summary>
    /// 创建自动前往充电位任务。
    /// </summary>
    private async Task CreateNavigationTaskAsync(Robot robot, ChargingPile pile, CancellationToken cancellationToken)
    {
        if (!robot.CurrentNodeId.HasValue || !pile.MapNodeId.HasValue)
        {
            return;
        }

        var protocol = _protocolServiceFactory.GetService(robot);
        var sendOrderResponse = await protocol.SendOrderWithTaskContextAsync(
            robot,
            priority: Math.Max(1, pile.Priority),
            endNodeId: pile.MapNodeId.Value,
            containerId: null,
            taskId: null,
            subTaskId: null,
            taskTemplateId: null,
            subTaskSequence: null,
            taskCode: null,
            mapId: robot.CurrentMapCodeId,
            ct: cancellationToken);

        if (!sendOrderResponse.Success)
        {
            _logger.LogWarning(
                "Auto charging order dispatch failed. Robot={RobotCode}, Pile={PileCode}, EndNode={EndNodeId}, Message={Message}",
                robot.RobotCode,
                pile.PileCode,
                pile.MapNodeId,
                sendOrderResponse.Message);
            return;
        }
    }
    /// <summary>
    /// 选择最近且未被占用的可用充电桩。
    /// </summary>
    private async Task<ChargingPile?> SelectChargingPileAsync(Robot robot, CancellationToken cancellationToken)
    {
        if (!robot.BatteryLevel.HasValue)
        {
            return null;
        }

        var allPiles = await _chargingPileRepository.GetQueryable()
            .Where(p => p.IsActive && p.MapNodeId.HasValue && !p.CurrentChargingRobotId.HasValue)
            .ToListAsync(cancellationToken);

        if (!allPiles.Any())
        {
            return null;
        }
        var nodeIds = allPiles
            .Where(p => p.MapNodeId.HasValue)
            .Select(p => p.MapNodeId!.Value)
            .Distinct()
            .ToList();

        // if (robot.CurrentNodeId.HasValue && !nodeIds.Contains(robot.CurrentNodeId.Value))
        // {
        //     nodeIds.Add(robot.CurrentNodeId.Value);
        // }

        var nodeLookup = await _mapNodeRepository.GetQueryable()
            .Where(n => nodeIds.Contains(n.NodeId))
            .ToDictionaryAsync(n => n.NodeId, cancellationToken);

        var hasRobotPosition = TryResolveRobotPosition(robot, nodeLookup, out var robotX, out var robotY);

        var candidates = new List<(ChargingPile Pile, double DistanceSquared)>();
        foreach (var pile in allPiles)
        {
            if (!pile.MapNodeId.HasValue)
            {
                continue;
            }

            if (robot.BatteryLevel.Value > (double)pile.AutoStartThreshold)
            {
                continue;
            }

            if (!nodeLookup.TryGetValue(pile.MapNodeId.Value, out var mapNode) || !mapNode.Active || mapNode.Type != MapNodeTYPE.charger)
            {
                continue;
            }

            if (robot.CurrentMapCodeId.HasValue && mapNode.MapId != robot.CurrentMapCodeId.Value)
            {
                continue;
            }

            if (pile.BoundRobotIds.Any() && !pile.BoundRobotIds.Contains(robot.RobotId))
            {
                continue;
            }
            if (pile.SupportedRobotModels.Any() && !pile.SupportedRobotModels.Contains(robot.RobotModel ?? string.Empty, StringComparer.OrdinalIgnoreCase))
            {
                continue;
            }
            // 暂时使用直线距离来评估距离，后续可以考虑接入路径规划服务来获取更准确的导航距离。
            var distanceSquared = hasRobotPosition
                ? CalculateDistanceSquared(robotX, robotY, mapNode.X, mapNode.Y)
                : double.MaxValue;

            candidates.Add((pile, distanceSquared));
        }

        return candidates
            .OrderBy(x => x.DistanceSquared)
            .ThenBy(x => x.Pile.Priority)
            .ThenBy(x => x.Pile.PileCode)
            .Select(x => x.Pile)
            .FirstOrDefault();
    }
    /// <summary>
    /// 获取被其他机器人占用的充电桩节点集合。
    /// </summary>
    private async Task<HashSet<Guid>> GetOccupiedPileNodeIdsByOtherRobotsAsync(
        Guid currentRobotId,
        IEnumerable<Guid> pileNodeIds,
        CancellationToken cancellationToken)
    {
        var nodeIdList = pileNodeIds.Distinct().ToList();
        if (!nodeIdList.Any())
        {
            return new HashSet<Guid>();
        }

        var occupiedNodeIds = await _robotRepository.GetQueryable()
            .Where(r =>
                r.Active
                && r.RobotId != currentRobotId
                && r.CurrentNodeId.HasValue
                && nodeIdList.Contains(r.CurrentNodeId.Value))
            .Select(r => r.CurrentNodeId!.Value)
            .Distinct()
            .ToListAsync(cancellationToken);

        return occupiedNodeIds.ToHashSet();
    }
    /// <summary>
    /// 解析机器人当前位置坐标。
    /// </summary>
    private static bool TryResolveRobotPosition(Robot robot, IReadOnlyDictionary<Guid, MapNode> nodeLookup, out double x, out double y)
    {
        if (robot.CurrentX.HasValue && robot.CurrentY.HasValue)
        {
            x = robot.CurrentX.Value;
            y = robot.CurrentY.Value;
            return true;
        }

        if (robot.CurrentNodeId.HasValue && nodeLookup.TryGetValue(robot.CurrentNodeId.Value, out var currentNode))
        {
            x = currentNode.X;
            y = currentNode.Y;
            return true;
        }

        x = 0d;
        y = 0d;
        return false;
    }
    /// <summary>
    /// 计算两点距离平方。
    /// </summary>
    private static double CalculateDistanceSquared(double x1, double y1, double x2, double y2)
    {
        var dx = x1 - x2;
        var dy = y1 - y2;
        return (dx * dx) + (dy * dy);
    }
    /// <summary>
    /// 按动作配置解析动作名称。
    /// </summary>
    private async Task<string> ResolveActionNameAsync(
        Robot robot,
        ActionCategory category,
        string defaultActionName,
        CancellationToken cancellationToken)
    {
        var configs = await _actionConfigurationRepository.GetByManufacturerAndTypeAsync(
            robot.RobotManufacturer,
            robot.RobotType,
            category,
            cancellationToken);

        var config = configs
            .Where(a => a.IsEnabled && !string.IsNullOrWhiteSpace(a.ActionName))
            .OrderBy(a => a.SortOrder)
            .ThenBy(a => a.ActionName)
            .FirstOrDefault();

        return config?.ActionName?.Trim() ?? defaultActionName;
    }
    /// <summary>
    /// 判断是否满足停止充电条件。
    /// </summary>
    private static bool ShouldStopCharging(Robot robot, ChargingPile pile)
    {
        return robot.BatteryLevel.HasValue
               && robot.BatteryLevel.Value >= (double)pile.FullChargeThreshold;
    }
    /// <summary>
    /// 获取机器人活跃任务列表。
    /// </summary>
    private async Task<List<RobotTask>> GetActiveRobotTasksAsync(Guid robotId, CancellationToken cancellationToken)
    {
        var tasks = await _robotTaskRepository.GetByRobotIdAsync(robotId, cancellationToken);
        return tasks.Where(IsTaskActive).ToList();
    }
    /// <summary>
    /// 判断任务是否为自动充电任务。
    /// </summary>
    private static bool IsAutoChargeTask(RobotTask task)
    {
        return string.Equals(task.Source, AutoChargeSource, StringComparison.OrdinalIgnoreCase);
    }
    /// <summary>
    /// 判断任务是否处于活跃状态。
    /// </summary>
    private static bool IsTaskActive(RobotTask task)
    {
        return task.Status != TaskStatus.Completed
               && task.Status != TaskStatus.Cancelled
               && task.Status != TaskStatus.Failed
               && task.Status != TaskStatus.Timeout;
    }
}