StandardSence/StandardScene.MagCarSimulator/Map/MapPathFinder.cs

using System.Collections.Generic;
using System.Text;

namespace StandardScene.MagCarSimulator
{
    public static class MapPathFinder
    {
        /// <summary>
        /// 生成往返循环：起点→终点→起点（终点不重复，回到起点靠下标回绕）。
        /// </summary>
        public static List<int> BuildLoop(SimpleLiteMap map, int startSiteId, int endSiteId)
        {
            if (map == null)
            {
                throw new System.InvalidOperationException("尚未加载地图");
            }

            if (startSiteId == endSiteId)
            {
                throw new System.InvalidOperationException($"起点和终点不能相同（站点 {startSiteId}）");
            }

            if (!map.Sites.ContainsKey(startSiteId))
            {
                throw new System.InvalidOperationException($"地图中没有起点 {startSiteId}");
            }

            if (!map.Sites.ContainsKey(endSiteId))
            {
                throw new System.InvalidOperationException($"地图中没有终点 {endSiteId}");
            }

            var forward = FindPath(map, startSiteId, endSiteId);
            if (forward == null)
            {
                throw new System.InvalidOperationException($"找不到路径 {startSiteId} → {endSiteId}");
            }

            var back = FindPath(map, endSiteId, startSiteId);
            if (back == null)
            {
                throw new System.InvalidOperationException($"找不到返回路径 {endSiteId} → {startSiteId}");
            }

            var loop = new List<int>(forward.Count + back.Count);
            loop.AddRange(forward);
            for (var i = 1; i < back.Count - 1; i++)
            {
                loop.Add(back[i]);
            }

            if (loop.Count < 2)
            {
                throw new System.InvalidOperationException("循环路径至少需要两个站点");
            }

            return loop;
        }

        public static List<int> NormalizeLoop(IEnumerable<int> siteIds)
        {
            var loop = new List<int>();
            if (siteIds == null)
            {
                return loop;
            }

            foreach (var id in siteIds)
            {
                if (id <= 0)
                {
                    continue;
                }

                if (loop.Count > 0 && loop[loop.Count - 1] == id)
                {
                    continue;
                }

                loop.Add(id);
            }

            if (loop.Count >= 2 && loop[0] == loop[loop.Count - 1])
            {
                loop.RemoveAt(loop.Count - 1);
            }

            return loop;
        }

        public static List<int> ResolveLoop(SimpleLiteMap map, MagCarSimVehicleConfig config)
        {
            if (map == null)
            {
                throw new System.InvalidOperationException("尚未加载地图");
            }

            if (config?.LoopSiteIds != null && config.LoopSiteIds.Count >= 3)
            {
                var loop = NormalizeLoop(config.LoopSiteIds);
                if (loop.Count < 3)
                {
                    throw new System.InvalidOperationException($"{config.Name} 环线站点不足");
                }

                foreach (var id in loop)
                {
                    if (!map.Sites.ContainsKey(id))
                    {
                        throw new System.InvalidOperationException($"{config.Name} 环线站点 {id} 不在地图中");
                    }
                }

                return loop;
            }

            return BuildLoop(map, config.StartSiteId, config.EndSiteId);
        }

        public static string FormatPath(IReadOnlyList<int> siteIds, int maxShow = 0)
        {
            if (siteIds == null || siteIds.Count == 0)
            {
                return "";
            }

            var show = siteIds.Count;
            var truncated = false;
            if (maxShow > 0 && siteIds.Count > maxShow)
            {
                show = maxShow;
                truncated = true;
            }

            var sb = new StringBuilder();
            for (var i = 0; i < show; i++)
            {
                if (i > 0)
                {
                    sb.Append("→");
                }

                sb.Append(siteIds[i]);
            }

            if (truncated)
            {
                sb.Append("→…共").Append(siteIds.Count).Append("站");
            }
            else
            {
                sb.Append("→").Append(siteIds[0]);
            }

            return sb.ToString();
        }

        public static List<int> FindPath(SimpleLiteMap map, int fromSiteId, int toSiteId)
        {
            if (fromSiteId == toSiteId)
            {
                return new List<int> { fromSiteId };
            }

            var queue = new Queue<int>();
            var prev = new Dictionary<int, int>();
            var visited = new HashSet<int> { fromSiteId };
            queue.Enqueue(fromSiteId);

            while (queue.Count > 0)
            {
                var current = queue.Dequeue();
                if (!map.Adjacency.TryGetValue(current, out var nexts))
                {
                    continue;
                }

                foreach (var next in nexts)
                {
                    if (!visited.Add(next))
                    {
                        continue;
                    }

                    prev[next] = current;
                    if (next == toSiteId)
                    {
                        return Reconstruct(prev, fromSiteId, toSiteId);
                    }

                    queue.Enqueue(next);
                }
            }

            return null;
        }

        private static List<int> Reconstruct(Dictionary<int, int> prev, int fromSiteId, int toSiteId)
        {
            var path = new List<int>();
            var current = toSiteId;
            path.Add(current);
            while (current != fromSiteId)
            {
                current = prev[current];
                path.Add(current);
            }

            path.Reverse();
            return path;
        }
    }
}