StandardSence/StandardScene.Magnetic/Tasking/Fass2TaskBuilder.cs

using SimpleCore;
using SimpleCore.PropType;
using StandardScene.Magnetic.Protocol;
using System;
using System.Collections.Generic;

namespace StandardScene.Magnetic.Tasking
{
    /// <summary>
    /// 将站点路径与站点/边 fields 合并为 FASS 2.0 <c>0xB1</c> 节点序列。
    /// 对齐 backend <c>CarRequestService.GetSendNodes</c>：边属性挂在节点<b>出边</b>上。
    /// </summary>
    public static class Fass2TaskBuilder
    {
        public const byte StartStopPass = 1;
        public const byte StartStopStop = 2;
        public const byte StartStopControlStart = 11;
        public const byte StartStopControlStop = 12;
        public const byte StartStopPrecision = 22;

        public static Fass2TaskPlan BuildPath(int startSiteId, int goalSiteId, Fass2TaskBuildOptions options,
            Func<int, ushort> resolveNodeId, AbstractCar routingCar = null)
        {
            if (resolveNodeId == null)
            {
                throw new ArgumentNullException(nameof(resolveNodeId));
            }

            options ??= new Fass2TaskBuildOptions();
            var siteIds = Fass2RouteHelper.GetSitesBetween(startSiteId, goalSiteId, routingCar);
            if (siteIds.Count == 0)
            {
                throw new InvalidOperationException($"no path from site {startSiteId} to {goalSiteId}");
            }

            var nodes = BuildNodesForSites(siteIds, options, resolveNodeId);
            return new Fass2TaskPlan
            {
                StartSiteId = startSiteId,
                GoalSiteId = goalSiteId,
                SiteIds = siteIds,
                Nodes = nodes,
                Batches = SplitBatches(nodes, options.MaxNodesPerFrame),
                FieldsSignature = Fass2SiteFieldReader.BuildFieldsSignature(siteIds, 0, SimpleLib.GetSite)
            };
        }

        public static Fass2NodeMessage[] BuildSegment(int srcSiteId, int dstSiteId, Fass2TaskBuildOptions options,
            Func<int, ushort> resolveNodeId, AbstractCar routingCar = null)
        {
            var plan = BuildPath(srcSiteId, dstSiteId, options, resolveNodeId, routingCar);
            var nodes = plan.Nodes;
            var array = new Fass2NodeMessage[nodes.Count];
            for (var i = 0; i < nodes.Count; i++)
            {
                array[i] = nodes[i];
            }

            return array;
        }

        public static IReadOnlyList<Fass2NodeMessage[]> SplitBatches(IReadOnlyList<Fass2NodeMessage> nodes, int maxPerFrame = 10)
        {
            if (nodes == null || nodes.Count == 0)
            {
                return Array.Empty<Fass2NodeMessage[]>();
            }

            if (maxPerFrame <= 0 || maxPerFrame > 10)
            {
                maxPerFrame = 10;
            }

            var batches = new List<Fass2NodeMessage[]>();
            for (var offset = 0; offset < nodes.Count; offset += maxPerFrame)
            {
                var count = Math.Min(maxPerFrame, nodes.Count - offset);
                var batch = new Fass2NodeMessage[count];
                for (var i = 0; i < count; i++)
                {
                    batch[i] = nodes[offset + i];
                }

                batches.Add(batch);
            }

            return batches;
        }

        private static List<Fass2NodeMessage> BuildNodesForSites(IReadOnlyList<int> siteIds, Fass2TaskBuildOptions options,
            Func<int, ushort> resolveNodeId)
        {
            var nodes = new List<Fass2NodeMessage>(siteIds.Count);
            for (var i = 0; i < siteIds.Count; i++)
            {
                var siteId = siteIds[i];
                var site = SimpleLib.GetSite(siteId);
                var siteAction = Fass2SiteFieldReader.Read(site);
                var isLast = i == siteIds.Count - 1;
                var nextSiteId = isLast ? -1 : siteIds[i + 1];

                Fass2TrackMotionData trackMotion = null;
                Site currentSite = site;
                Site nextSite = null;
                if (!isLast)
                {
                    nextSite = SimpleLib.GetSite(nextSiteId);
                    var track = Fass2RouteHelper.FindTrack(siteId, nextSiteId);
                    trackMotion = Fass2TrackFieldReader.Read(track);
                }

                var node = new Fass2NodeMessage
                {
                    Node = resolveNodeId(siteId),
                    StartStop = ResolveStartStop(siteAction, isLast),
                    Distance = !isLast ? ComputeEdgeDistance(currentSite, nextSite) : (ushort)0,
                    Speed = !isLast
                        ? SpeedToProtocol(trackMotion?.Speed ?? options.DefaultSpeed)
                        : (ushort)0,
                    Orientation = !isLast ? ResolveOrientation(currentSite, nextSite, trackMotion) : (byte)0,
                    Byroad = siteAction.Byroad ?? trackMotion?.Byroad ?? 0,
                    Direction = siteAction.Direction ?? trackMotion?.Direction ?? 0,
                    Lift = siteAction.Lift ?? 0,
                    Roll = siteAction.Roll ?? 0,
                    Charge = siteAction.Charge ?? 0,
                    Obstacle = siteAction.Obstacle ?? 0,
                    Audio = siteAction.Audio ?? 0,
                    Light = siteAction.Light ?? 0,
                    Rest = siteAction.Rest ?? 0,
                    Clamp = siteAction.Clamp ?? 0,
                    Tray = siteAction.Tray ?? 0,
                    Shutdown = siteAction.Shutdown ?? 0
                };

                if (siteAction.Orientation.HasValue)
                {
                    node.Orientation = siteAction.Orientation.Value;
                }

                nodes.Add(node);
            }

            return nodes;
        }

        private static byte ResolveStartStop(Fass2SiteActionData siteAction, bool isLast)
        {
            if (siteAction.StartStop.HasValue)
            {
                return siteAction.StartStop.Value;
            }

            if (isLast)
            {
                return siteAction.PrecisionStop ? StartStopPrecision : StartStopStop;
            }

            return StartStopPass;
        }

        private static byte ResolveOrientation(Site src, Site dst, Fass2TrackMotionData trackMotion)
        {
            if (trackMotion?.Orientation != null)
            {
                return trackMotion.Orientation.Value;
            }

            if (src == null || dst == null)
            {
                return 0;
            }

            var angle = trackMotion != null && trackMotion.EnableCarAbsoluteDirection
                ? trackMotion.CarAbsoluteDirection
                : Math.Atan2(dst.y - src.y, dst.x - src.x) / Math.PI * 180.0;

            if (trackMotion?.Reverse == true)
            {
                angle += 180;
            }

            angle += trackMotion?.CarDirectionBias ?? 0;
            angle %= 360;
            if (angle < 0)
            {
                angle += 360;
            }

            return (byte)((int)Math.Round(angle / 360.0 * 256) % 256);
        }

        private static ushort ComputeEdgeDistance(Site src, Site dst)
        {
            var dx = dst.x - src.x;
            var dy = dst.y - src.y;
            var length = Math.Sqrt(dx * dx + dy * dy);
            return (ushort)Math.Min(ushort.MaxValue, Math.Max(1, Math.Round(length)));
        }

        /// <summary>
        /// 协议速度单位 0.1 m/min；地图 Speed 默认 m/min，≤1 视为旧版 m/s 兼容。
        /// </summary>
        public static ushort SpeedToProtocol(double speed)
        {
            if (speed > 0 && speed <= 1)
            {
                speed *= 60;
            }

            return (ushort)Math.Max(1, Math.Min(10000, Math.Round(speed * 10)));
        }
    }
}