StandardSence/StandardScene.Magnetic/Tasking/Fass2TaskBuilder.cs

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

namespace StandardScene.Magnetic.Tasking
{
    /// <summary>
    /// 将站点路径与站点/边 fields 合并为 FASS 2.0 <c>0xB1</c> 节点序列。
    /// 对齐 backend <c>CarRequestService.GetSendNodes</c> 的组包规则。
    /// </summary>
    public static class Fass2TaskBuilder
    {
        public const byte StartStopPass = 1;
        public const byte StartStopStop = 2;
        public const byte StartStopPrecision = 22;

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

            options ??= new Fass2TaskBuildOptions();
            var siteIds = Fass2PathFinder.GetSitesBetween(startSiteId, goalSiteId);
            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)
        {
            return BuildPath(srcSiteId, dstSiteId, options, resolveNodeId).Nodes.ToArray();
        }

        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 prevSiteId = i > 0 ? siteIds[i - 1] : siteId;

                Fass2TrackMotionData trackMotion = null;
                Site prevSite = null;
                if (i > 0)
                {
                    prevSite = SimpleLib.GetSite(prevSiteId);
                    var track = FindTrack(prevSiteId, siteId);
                    trackMotion = Fass2TrackFieldReader.Read(track);
                }

                var node = new Fass2NodeMessage
                {
                    Node = resolveNodeId(siteId),
                    StartStop = ResolveStartStop(siteAction, isLast),
                    Distance = i > 0 ? ComputeEdgeDistance(prevSite, site) : (ushort)0,
                    Speed = SpeedToProtocol(trackMotion?.Speed ?? options.DefaultSpeed, options.CarSpeed),
                    Orientation = ResolveOrientation(prevSite, site, trackMotion),
                    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)));
        }

        private static ushort SpeedToProtocol(double trackSpeed, double carSpeed)
        {
            var value = trackSpeed <= 0 ? carSpeed : trackSpeed;
            var protocolSpeed = value <= 1
                ? (int)Math.Round(value * 600)
                : (int)Math.Round(value * 10);
            return (ushort)Math.Max(1, Math.Min(10000, protocolSpeed));
        }

        private static Track FindTrack(int fromSiteId, int toSiteId)
        {
            foreach (var track in SimpleLib.GetAllTracks())
            {
                if (track.direction == 0)
                {
                    if ((track.siteA == fromSiteId && track.siteB == toSiteId) ||
                        (track.siteB == fromSiteId && track.siteA == toSiteId))
                    {
                        return track;
                    }

                    continue;
                }

                if (track.direction == 1 && track.siteA == fromSiteId && track.siteB == toSiteId)
                {
                    return track;
                }

                if (track.direction == 2 && track.siteB == fromSiteId && track.siteA == toSiteId)
                {
                    return track;
                }
            }

            return null;
        }
    }
}