StandardSence/StandardScene.Magnetic/Tasking/Fass2RouteHelper.cs

using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.Library;
using SimpleCore.PropType;
using System;
using System.Collections.Generic;

namespace StandardScene.Magnetic.Tasking
{
    /// <summary>
    /// 从站点序列构建 <see cref="SegmentPlan"/>，供交管 Forecast 与任务下发共用同一路径。
    /// </summary>
    internal static class Fass2RouteHelper
    {
        public static List<int> GetSitesBetween(int startSiteId, int endSiteId, AbstractCar car)
        {
            if (startSiteId == endSiteId)
            {
                return new List<int> { startSiteId };
            }

            if (car != null)
            {
                var startSite = SimpleLib.GetSite(startSiteId);
                var endSite = SimpleLib.GetSite(endSiteId);
                if (startSite != null && endSite != null)
                {
                    var plan = new SegmentPlan { usingCar = car };
                    plan.fields["allow_destination_on_route"] = "true";
                    plan.FindRoute(startSite, endSite);
                    var sites = ExtractSiteIds(plan);
                    if (sites.Count > 0)
                    {
                        return sites;
                    }
                }
            }

            return Fass2PathFinder.GetSitesBetweenBfs(startSiteId, endSiteId);
        }

        public static void ForecastTrafficSequence(AbstractCar car, IReadOnlyList<int> siteIds, int fromIndex)
        {
            if (car == null || siteIds == null || siteIds.Count == 0)
            {
                throw new ArgumentException("traffic forecast requires car and siteIds");
            }

            fromIndex = Math.Max(0, Math.Min(fromIndex, siteIds.Count - 1));
            if (fromIndex >= siteIds.Count - 1)
            {
                return;
            }

            var routeSiteIds = new List<int>();
            for (var i = fromIndex; i < siteIds.Count; i++)
            {
                routeSiteIds.Add(siteIds[i]);
            }

            try
            {
                var plan = BuildSegmentPlan(car, routeSiteIds);
                var program = plan.Compile("fass2-traffic", false);
                program.Forecast();
            }
            catch (Exception ex) when (IsRecoverableForecastFailure(ex))
            {
                // 双车 Multi-car search Exhausted 时降级：只建 pending/seqScope，交由 TryLock 现场占点
                ApplyManualTrafficSequence(car, routeSiteIds);
                Diagnosis.Post(
                    $"Mag2Car forecast fallback manual sequence car={car.id}: {ex.Message}",
                    "Mag2Car",
                    true);
            }
        }

        /// <summary>
        /// 不走 SimpleCore 多车 Forecast，直接写入交管序列（持锁起点已由 TrafficReset 保证）。
        /// </summary>
        public static void ApplyManualTrafficSequence(AbstractCar car, IReadOnlyList<int> routeSiteIds)
        {
            if (car == null || routeSiteIds == null || routeSiteIds.Count == 0)
            {
                return;
            }

            var scope = new int[routeSiteIds.Count];
            for (var i = 0; i < routeSiteIds.Count; i++)
            {
                scope[i] = routeSiteIds[i];
            }

            var pending = new int[Math.Max(0, scope.Length - 1)];
            for (var i = 0; i < pending.Length; i++)
            {
                pending[i] = scope[i + 1];
            }

            car.status.seqScope = scope;
            car.status.pendingLocks = pending;
            car.status.seqPtr = scope.Length > 1 ? 1 : 0;
            car.status.escape = Array.Empty<int>();
        }

        private static bool IsRecoverableForecastFailure(Exception ex)
        {
            for (var cur = ex; cur != null; cur = cur.InnerException)
            {
                var msg = cur.Message ?? string.Empty;
                if (msg.IndexOf("Multi-car traffic search", StringComparison.OrdinalIgnoreCase) >= 0 ||
                    msg.IndexOf("Exhausted", StringComparison.OrdinalIgnoreCase) >= 0 ||
                    msg.IndexOf("BadForecast", StringComparison.OrdinalIgnoreCase) >= 0 ||
                    cur.GetType().Name.IndexOf("Forecast", StringComparison.OrdinalIgnoreCase) >= 0)
                {
                    return true;
                }
            }

            return false;
        }

        public static SegmentPlan BuildSegmentPlan(AbstractCar car, IReadOnlyList<int> siteIds)
        {
            if (car == null || siteIds == null || siteIds.Count == 0)
            {
                throw new ArgumentException("segment plan requires car and siteIds");
            }

            var plan = new SegmentPlan { usingCar = car };
            plan.fields["allow_destination_on_route"] = "true";

            var firstSite = SimpleLib.GetSite(siteIds[0]);
            if (firstSite == null)
            {
                throw new InvalidOperationException($"site {siteIds[0]} not found");
            }

            plan.segments.Add(firstSite);

            for (var i = 0; i < siteIds.Count - 1; i++)
            {
                var dstSite = SimpleLib.GetSite(siteIds[i + 1]);
                if (dstSite == null)
                {
                    throw new InvalidOperationException($"site {siteIds[i + 1]} not found");
                }

                var track = FindTrack(siteIds[i], siteIds[i + 1]);
                if (track == null)
                {
                    throw new InvalidOperationException($"no track from site {siteIds[i]} to {siteIds[i + 1]}");
                }

                plan.segments.Add(track);
                plan.segments.Add(dstSite);
            }

            return plan;
        }

        public static List<int> ExtractSiteIds(SegmentPlan plan)
        {
            var sites = new List<int>();
            if (plan?.segments == null)
            {
                return sites;
            }

            foreach (var segment in plan.segments)
            {
                if (segment is Site site)
                {
                    sites.Add(site.id);
                }
            }

            return sites;
        }

        public 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;
        }
    }
}