zengzy / hhrcs_service · Files

using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
using Rcs.Domain.Entities;
using Rcs.Domain.Enums;
using Rcs.Domain.Settings;
using Microsoft.Extensions.Options;

namespace Rcs.Infrastructure.PathFinding.Services;

/// <summary>
/// VDA 路径切割服务实现。
/// </summary>
public sealed class VdaPathSegmentationService : IVdaPathSegmentationService
{
    /// <summary>
    /// 单次下发路径的最小节点数限制（至少包含起点和终点）。
    /// </summary>
    private const int MinDispatchNodeCount = 2;

    /// <summary>
    /// 需要进行切割的资源类型集合。
    /// </summary>
    private static readonly HashSet<int> CuttingResourceTypes = new()
    {
        (int)MapResourcesTYPE.dock,
        (int)MapResourcesTYPE.charger,
        (int)MapResourcesTYPE.elevator,
        (int)MapResourcesTYPE.airlock,
        (int)MapResourcesTYPE.conveyor,
        (int)MapResourcesTYPE.storage
    };

    private readonly IOptionsMonitor<AppSettings> _settingsMonitor;

    public VdaPathSegmentationService(IOptionsMonitor<AppSettings> settingsMonitor)
    {
        _settingsMonitor = settingsMonitor;
    }

    /// <inheritdoc />
    public List<List<List<PathSegmentWithCode>>> SplitSegmentsByBoundary(
        List<PathSegmentWithCode> segments,
        PathGraph graph,
        Robot robot,
        bool hasTerminalAction)
    {
        var result = new List<List<List<PathSegmentWithCode>>>();
        if (segments.Count == 0)
        {
            return result;
        }

        var junctionNodes = GetJunctionNodes(graph);
        var resourceBoundaryCutNodes = GetResourceBoundaryCutNodes(segments, graph, junctionNodes);
        var junctionSegments = SplitByJunctions(segments, graph, robot);

        foreach (var junctionGroup in junctionSegments)
        {
            var resourceSegments = SplitByResourceBoundary(junctionGroup, resourceBoundaryCutNodes);
            result.Add(resourceSegments);
        }

        if (hasTerminalAction)
        {
            EnsureTerminalSegmentIsolated(result);
        }

        var maxDispatchNodeCount = GetMaxDispatchNodeCount();
        return EnforceMaxDispatchNodeCount(result, maxDispatchNodeCount);
    }

    private int GetMaxDispatchNodeCount()
    {
        var configuredCount = _settingsMonitor.CurrentValue.Vda5050.MaxDispatchNodeCount;
        return Math.Max(MinDispatchNodeCount, configuredCount);
    }

    private static List<List<List<PathSegmentWithCode>>> EnforceMaxDispatchNodeCount(
        List<List<List<PathSegmentWithCode>>> segmentedPath,
        int maxDispatchNodeCount)
    {
        if (segmentedPath.Count == 0)
        {
            return segmentedPath;
        }

        var maxSegmentsPerDispatch = Math.Max(1, maxDispatchNodeCount - 1);
        var normalized = new List<List<List<PathSegmentWithCode>>>(segmentedPath.Count);

        foreach (var junctionGroup in segmentedPath)
        {
            var normalizedResourceGroups = new List<List<PathSegmentWithCode>>();
            foreach (var resourceGroup in junctionGroup)
            {
                if (resourceGroup.Count <= maxSegmentsPerDispatch)
                {
                    normalizedResourceGroups.Add(resourceGroup);
                    continue;
                }

                for (var i = 0; i < resourceGroup.Count; i += maxSegmentsPerDispatch)
                {
                    normalizedResourceGroups.Add(resourceGroup.Skip(i).Take(maxSegmentsPerDispatch).ToList());
                }
            }

            if (normalizedResourceGroups.Count > 0)
            {
                normalized.Add(normalizedResourceGroups);
            }
        }

        return normalized;
    }

    private static void EnsureTerminalSegmentIsolated(List<List<List<PathSegmentWithCode>>> segmentedPath)
    {
        if (segmentedPath.Count == 0)
        {
            return;
        }

        var lastJunctionGroup = segmentedPath[^1];
        if (lastJunctionGroup.Count == 0)
        {
            return;
        }

        var lastResourceGroup = lastJunctionGroup[^1];
        if (lastResourceGroup.Count <= 1)
        {
            return;
        }

        var terminalSegment = lastResourceGroup[^1];
        lastResourceGroup.RemoveAt(lastResourceGroup.Count - 1);
        lastJunctionGroup.Add(new List<PathSegmentWithCode> { terminalSegment });
    }

    private static List<List<PathSegmentWithCode>> SplitByJunctions(
        List<PathSegmentWithCode> segments,
        PathGraph graph,
        Robot robot)
    {
        var result = new List<List<PathSegmentWithCode>>();
        if (segments.Count == 0)
        {
            return result;
        }

        var junctionNodes = GetJunctionNodes(graph);
        bool IsJunction(Guid nodeId) => junctionNodes.Contains(nodeId);
        var clearanceDistance = CalculateJunctionRotationRange(robot);
        var cutIndices = new List<int>();

        for (var i = 0; i < segments.Count; i++)
        {
            var seg = segments[i];
            var isToJunction = IsJunction(seg.ToNodeId);
            if (isToJunction && !IsJunction(seg.FromNodeId))
            {
                var cutIndex = GetJunctionCutIndexByRotationRange(segments, graph, junctionNodes, i, clearanceDistance);
                if (cutIndex.HasValue)
                {
                    cutIndices.Add(cutIndex.Value);
                }
            }
        }

        var startIndex = 0;
        foreach (var cutIndex in cutIndices)
        {
            if (cutIndex >= startIndex && cutIndex < segments.Count)
            {
                var segmentLength = cutIndex - startIndex + 1;
                if (segmentLength > 0)
                {
                    result.Add(segments.Skip(startIndex).Take(segmentLength).ToList());
                }

                startIndex = cutIndex + 1;
            }
        }

        if (startIndex < segments.Count)
        {
            result.Add(segments.Skip(startIndex).ToList());
        }

        if (result.Count == 0)
        {
            result.Add(segments.ToList());
        }

        return result;
    }

    private static double CalculateJunctionRotationRange(Robot robot)
    {
        var robotLength = Math.Max(robot.Length ?? 0d, 0d);
        var robotWidth = Math.Max(robot.Width ?? 0d, 0d);
        var safetyDistance = Math.Max(robot.SafetyDistance ?? 0d, 0d);
        var turningRadius = Math.Sqrt((robotLength * robotLength) + (robotWidth * robotWidth)) / 2d;
        return (turningRadius + safetyDistance) * 2d;
    }

    private static int? GetJunctionCutIndexByRotationRange(
        IReadOnlyList<PathSegmentWithCode> segments,
        PathGraph graph,
        HashSet<Guid> junctionNodes,
        int junctionEdgeIndex,
        double rotationRange)
    {
        if (junctionEdgeIndex <= 0)
        {
            return null;
        }

        if (rotationRange <= 0)
        {
            return junctionEdgeIndex - 1;
        }

        var junctionNodeId = segments[junctionEdgeIndex].ToNodeId;
        for (var i = junctionEdgeIndex; i >= 0; i--)
        {
            var candidateNodeId = segments[i].FromNodeId;
            if (junctionNodes.Contains(candidateNodeId))
            {
                continue;
            }

            var distance = GetNodeStraightLineDistance(candidateNodeId, junctionNodeId, graph);
            if (!distance.HasValue)
            {
                continue;
            }

            if (distance.Value > rotationRange)
            {
                if (i > 0)
                {
                    return i - 1;
                }

                return null;
            }
        }

        return null;
    }

    private static double? GetNodeStraightLineDistance(Guid fromNodeId, Guid toNodeId, PathGraph graph)
    {
        if (graph.Nodes.TryGetValue(fromNodeId, out var fromNode) &&
            graph.Nodes.TryGetValue(toNodeId, out var toNode))
        {
            var dx = toNode.X - fromNode.X;
            var dy = toNode.Y - fromNode.Y;
            var distance = Math.Sqrt((dx * dx) + (dy * dy));
            return Math.Max(distance, 0d);
        }

        return null;
    }

    private static List<List<PathSegmentWithCode>> SplitByResourceBoundary(
        List<PathSegmentWithCode> segments,
        HashSet<Guid> resourceBoundaryCutNodes)
    {
        var result = new List<List<PathSegmentWithCode>>();
        if (segments.Count == 0)
        {
            return result;
        }

        var cutIndices = new List<int>();
        for (var i = 0; i < segments.Count; i++)
        {
            var seg = segments[i];
            if (resourceBoundaryCutNodes.Contains(seg.ToNodeId))
            {
                cutIndices.Add(i);
            }
        }

        var startIndex = 0;
        foreach (var cutIndex in cutIndices)
        {
            if (cutIndex >= startIndex && cutIndex < segments.Count)
            {
                var segmentLength = cutIndex - startIndex + 1;
                if (segmentLength > 0)
                {
                    result.Add(segments.Skip(startIndex).Take(segmentLength).ToList());
                }

                startIndex = cutIndex + 1;
            }
        }

        if (startIndex < segments.Count)
        {
            result.Add(segments.Skip(startIndex).ToList());
        }

        if (result.Count == 0)
        {
            result.Add(segments.ToList());
        }

        return result;
    }

    private static HashSet<Guid> GetResourceBoundaryCutNodes(
        List<PathSegmentWithCode> segments,
        PathGraph graph,
        HashSet<Guid> junctionNodes)
    {
        var cutNodes = new HashSet<Guid>();
        if (segments.Count == 0)
        {
            return cutNodes;
        }

        var resourceLookup = BuildNodeResourceLookup(graph);
        var resourceByCode = graph.Resources
            .Where(r => CuttingResourceTypes.Contains(r.Type))
            .ToDictionary(r => r.ResourceCode, StringComparer.OrdinalIgnoreCase);

        void AddCutNode(Guid nodeId, int segIndex)
        {
            if (junctionNodes.Contains(nodeId))
            {
                for (var j = segIndex; j >= 0; j--)
                {
                    var candidateId = segments[j].FromNodeId;
                    if (!junctionNodes.Contains(candidateId))
                    {
                        cutNodes.Add(candidateId);
                        return;
                    }
                }
            }
            else
            {
                cutNodes.Add(nodeId);
            }
        }

        for (var i = 0; i < segments.Count; i++)
        {
            var seg = segments[i];
            var fromRes = resourceLookup.TryGetValue(seg.FromNodeId, out var fr) ? fr : new HashSet<string>();
            var toRes = resourceLookup.TryGetValue(seg.ToNodeId, out var tr) ? tr : new HashSet<string>();

            var enteringResources = toRes.Except(fromRes);
            foreach (var resCode in enteringResources)
            {
                if (!resourceByCode.TryGetValue(resCode, out var resource))
                {
                    continue;
                }

                if ((resource.PreAction1Type ?? ActionType.None) == ActionType.Apply)
                {
                    AddCutNode(seg.FromNodeId, i > 0 ? i - 1 : i);
                }

                if ((resource.PostAction1Type ?? ActionType.None) == ActionType.Apply)
                {
                    AddCutNode(seg.ToNodeId, i);
                }
            }

            var leavingResources = fromRes.Except(toRes);
            foreach (var resCode in leavingResources)
            {
                if (!resourceByCode.TryGetValue(resCode, out var resource))
                {
                    continue;
                }

                if ((resource.PreAction2Type ?? ActionType.None) == ActionType.Apply)
                {
                    AddCutNode(seg.FromNodeId, i > 0 ? i - 1 : i);
                }

                if ((resource.PostAction2Type ?? ActionType.None) == ActionType.Apply)
                {
                    AddCutNode(seg.ToNodeId, i);
                }
            }
        }

        return cutNodes;
    }

    private static HashSet<Guid> GetJunctionNodes(PathGraph graph)
    {
        var nodeNeighbors = new Dictionary<Guid, HashSet<Guid>>();
        var undirectedEdges = new HashSet<(Guid A, Guid B)>();

        foreach (var edge in graph.Edges.Values)
        {
            var a = edge.FromNodeId;
            var b = edge.ToNodeId;
            var key = a.CompareTo(b) <= 0 ? (a, b) : (b, a);
            if (!undirectedEdges.Add(key))
            {
                continue;
            }

            AddNeighbor(nodeNeighbors, edge.FromNodeId, edge.ToNodeId);
            AddNeighbor(nodeNeighbors, edge.ToNodeId, edge.FromNodeId);
        }

        return nodeNeighbors
            .Where(kv => kv.Value.Count >= 3)
            .Select(kv => kv.Key)
            .ToHashSet();
    }

    private static void AddNeighbor(Dictionary<Guid, HashSet<Guid>> dict, Guid nodeId, Guid neighborId)
    {
        if (!dict.TryGetValue(nodeId, out var set))
        {
            set = new HashSet<Guid>();
            dict[nodeId] = set;
        }

        set.Add(neighborId);
    }

    private static Dictionary<Guid, HashSet<string>> BuildNodeResourceLookup(PathGraph graph)
    {
        var lookup = new Dictionary<Guid, HashSet<string>>();
        var cuttingResources = graph.Resources
            .Where(r => CuttingResourceTypes.Contains(r.Type) && r.LocationCoordinates != null && r.LocationCoordinates.Count >= 3)
            .ToList();

        foreach (var node in graph.Nodes.Values)
        {
            var resourceCodes = new HashSet<string>(StringComparer.OrdinalIgnoreCase);
            foreach (var resource in cuttingResources)
            {
                if (IsPointInPolygon(node.X, node.Y, resource.LocationCoordinates!))
                {
                    resourceCodes.Add(resource.ResourceCode);
                }
            }

            if (resourceCodes.Count > 0)
            {
                lookup[node.NodeId] = resourceCodes;
            }
        }

        return lookup;
    }

    private static bool IsPointInPolygon(double x, double y, List<PointCache> polygon)
    {
        if (polygon == null || polygon.Count < 3)
        {
            return false;
        }

        var inside = false;
        var j = polygon.Count - 1;
        for (var i = 0; i < polygon.Count; i++)
        {
            var xi = polygon[i].X;
            var yi = polygon[i].Y;
            var xj = polygon[j].X;
            var yj = polygon[j].Y;
            if (((yi > y) != (yj > y)) && (x < (xj - xi) * (y - yi) / (yj - yi) + xi))
            {
                inside = !inside;
            }

            j = i;
        }

        return inside;
    }
}