using Ananta.Server.Configuration;

namespace Ananta.Server.Handlers.Game;

/// <summary>
/// 车辆路网（服务端侧 ZoneGraph 简化版）—— 给 T3-7 车流提供「车道 handle + 沿车道里程」。
///
/// 数据来自捐赠端 `zonegraph_lanes.csv`（WorldMap_Release，14171 条车道），
/// 由 `_gen_vehicle_lanes.py` 压成两份运行时表：
///   `Ananta.Server/Ananta.App/data/vehicle_lanes.csv`       = handle;bx;by;bz;ex;ey;ez;length;speed
///   `Ananta.Server/Ananta.App/data/vehicle_lane_links.csv`  = handle;to1;to2;...
///
/// ★ 为什么用捐赠端那份而不是 `_ref/lanes/vehicle-lane-links.csv`：
///   后者 Type==1 前向边只有 10911 条、只覆盖 56.9% 的车道，且是捐赠端 next 的**真子集**；
///   捐赠端 next 是多值后继，覆盖 13910/14171 = 98.2%，换道接缝中位 0.00m。
///
/// ★ 与 PedLaneGraph 的关键差别：客户端按 <c>DistanceAlongLane</c> 在**它本地的真实车道曲线**上
///   插值（MassTrafficInterpolationUtils），所以服务端不需要车道折线，只要里程单调推进。
///   这里的 bx/ez 只用于「玩家附近挑车道」和 despawn 判定。
/// </summary>
internal static class VehicleLaneGraph
{
    internal struct DriveState
    {
        internal int Lane;
        internal float Distance;
    }

    private sealed class Lane
    {
        internal float Bx, By, Bz, Ex, Ey, Ez;
        internal float Length;
        internal float Speed;
        internal int[] Next = [];

        /// <summary>
        /// 这条车道能不能用于<b>导航求解</b>（2026-09-30）＝它是否属于车道图的
        /// <b>最大强连通分量</b>（既能被开进来、也能开出去）。见 <see cref="MarkRoutableLanes"/>。
        /// ⛑ 只有 <c>TrySnapLane</c>（导航）在意它；<c>TryPickSpawn</c>（车流出生）不看，
        ///   后者用的是更宽松的「有后继」判据，改了会动摇 T3-7 已经调好的出生分布。
        /// </summary>
        internal bool Routable;

        /// <summary>
        /// ★★ 2026-10-03：「弦中点偏离真实车道」的<b>弓高</b>（米）。
        ///
        /// CSV 只给了车道<b>弦的两个端点</b>（bx/bz → ex/ez），真实车道是曲线，客户端按
        /// <c>DistanceAlongLane</c> 在它本地的曲线上插值。我们沿<b>直线弦</b>取点，弯道时
        /// 弦中点会落到路面之外 —— 召唤车「卡在人行道 / 花坛里」就是这个（实测一条 70m 弯车道
        /// 弦长 62.96m、弧长 70.02m，弦中点偏离真实路面约 10m）。
        ///
        /// 推导（把车道近似成圆弧）：
        ///   弧弦比 <c>r = Length / chord ≈ 1 + θ²/24</c> ⇒ 圆心角 <c>θ ≈ sqrt(24(r-1))</c>
        ///   弓高 <c>h ≈ chord · θ / 8</c>
        /// 弦上参数 t 处的横向偏离 <c>δ(t) ≈ 4·h·t·(1-t)</c>（t=0/1 为 0，t=0.5 最大为 h）。
        /// </summary>
        internal float Sagitta;
    }

    /// <summary>
    /// 算一条车道的<b>弓高</b>（弦中点相对真实曲线的最大横向偏离，米）。见 <see cref="Lane.Sagitta"/>。
    /// 直车道（弧长≈弦长）返回 0。
    /// </summary>
    private static float SagittaOf(Lane lane)
    {
        var dx = lane.Ex - lane.Bx;
        var dz = lane.Ez - lane.Bz;
        var chord = MathF.Sqrt(dx * dx + dz * dz);
        if (chord < 1e-3f || lane.Length <= chord)
            return 0f;

        // 弧弦比 r ≈ 1 + θ²/24 ⇒ θ ≈ sqrt(24(r-1))；弓高 h ≈ chord·θ/8
        var ratio = lane.Length / chord;
        var theta = MathF.Sqrt(24f * (ratio - 1f));
        return chord * theta / 8f;
    }

    /// <summary>
    /// 弦上参数 <c>t</c>（0..1）处的横向偏离（米）。t=0/1 落在端点 ⇒ 偏离 0；t=0.5 最大 ⇒ 等于弓高。
    /// </summary>
    private static float LateralDeviationAt(float sagitta, float t)
        => sagitta <= 0f ? 0f : 4f * sagitta * t * (1f - t);

    private const float CellSize = 100f;

    private sealed class Table
    {
        internal Dictionary<int, Lane> Lanes = [];
        internal Dictionary<(int, int), List<int>> Cells = [];
        internal bool Ok;
        internal int RoutableLanes;
        internal int RoutableComponents;
    }

    private static readonly Lazy<Table> Cache = new(Load);

    internal static bool Available => Cache.Value.Ok;
    internal static int LaneCount => Cache.Value.Lanes.Count;

    /// <summary>最大强连通分量里的车道数（导航只会吸附到这些车道上）。排障用。</summary>
    internal static int RoutableLaneCount => Cache.Value.RoutableLanes;

    /// <summary>强连通分量个数。理想值 1；2026-09-30 实测 286（见 <see cref="MarkRoutableLanes"/>）。</summary>
    internal static int ComponentCount => Cache.Value.RoutableComponents;

    private static Table Load()
    {
        var table = new Table();
        var lanePath = PrivateServerConfigStore.ResolveProjectPath(
            "Ananta.Server/Ananta.App/data/vehicle_lanes.csv");
        var linkPath = PrivateServerConfigStore.ResolveProjectPath(
            "Ananta.Server/Ananta.App/data/vehicle_lane_links.csv");
        if (!File.Exists(lanePath) || !File.Exists(linkPath))
            return table;

        foreach (var line in File.ReadLines(lanePath))
        {
            if (line.Length == 0)
                continue;
            var parts = line.Split(';');
            if (parts.Length < 8 || !int.TryParse(parts[0], out var handle))
                continue;

            var lane = new Lane
            {
                Bx = Num(parts[1]), By = Num(parts[2]), Bz = Num(parts[3]),
                Ex = Num(parts[4]), Ey = Num(parts[5]), Ez = Num(parts[6]),
                Length = Num(parts[7]),
                Speed = parts.Length >= 9 ? Num(parts[8]) : 9.72f,
            };
            if (lane.Length <= 0.5f)
            {
                // 退化车道：按端点距离补一个下限，避免除零。
                var dx = lane.Ex - lane.Bx;
                var dz = lane.Ez - lane.Bz;
                lane.Length = MathF.Max(1f, MathF.Sqrt(dx * dx + dz * dz));
            }
            lane.Sagitta = SagittaOf(lane);
            table.Lanes[handle] = lane;

            var cell = CellOf(lane.Bx, lane.Bz);
            if (!table.Cells.TryGetValue(cell, out var bucket))
                table.Cells[cell] = bucket = [];
            bucket.Add(handle);
        }

        foreach (var line in File.ReadLines(linkPath))
        {
            if (line.Length == 0)
                continue;
            var parts = line.Split(';');
            if (parts.Length < 2 || !int.TryParse(parts[0], out var handle))
                continue;
            if (!table.Lanes.TryGetValue(handle, out var lane))
                continue;
            var outs = new List<int>(parts.Length - 1);
            for (var i = 1; i < parts.Length; i++)
                if (int.TryParse(parts[i], out var to) && table.Lanes.ContainsKey(to) && to != handle)
                    outs.Add(to);
            lane.Next = outs.ToArray();
        }

        MarkRoutableLanes(table);
        table.Ok = table.Lanes.Count > 0;
        return table;
    }

    // -----------------------------------------------------------------------------------------
    // ⛑⛑ 2026-09-30：车道图有 **285 条孤岛车道**，这是「自动驾驶开不到自定义标点」的真因。
    //
    // 体检脚本 `_vehicle_lane_graph_check.py` 的结果（可直接复跑）：
    //   车道 14171 条、链接表只覆盖 13910 条 ⇒ **261 条没有任何后继、255 条没有任何前驱**，
    //   两者的样例 id 完全重合（89/93/135/140/232/236/410/417…）⇒ 它们是<b>双向断开的孤儿车道</b>。
    //   Tarjan 的结果更狠：全图 **286 个强连通分量**，最大的那个有 13886 条（97%），
    //   剩下 285 条各自成形单影只的分量。
    //
    // 后果：日志里 `车道图不通 start=1795 target=1972` 反复出现且**永远不解开** —— 因为 lane 1972 /
    // 6901 / 13108 的「能到它的车道数」= 1（只有它自己）。而 <c>SolveRoute</c> 的 Dijkstra 只走前向
    // 后继，玩家把目的地标点落到这种车道附近时，**永远解不出路** ⇒ Start 之后根本没有 cruise 下发
    // ⇒ 现象就是「按了自动驾驶没反应」。
    // 商店/POI 之所以大多没事：它们的 endPos 是走 `GpsAreaConnectMgr` 算出来的**路口/出入口点**，
    // 天然落在主干道上；自定义标点给的是玩家自己点的原始坐标，撞上孤儿车道就是纯运气。
    //
    // ⇒ 治法：**导航吸附时只认最大强连通分量里的车道**。孤儿车道本来就没法参与路径，
    //   拿它当起点/终点必然失败；跳过它去找次近的正常车道，代价是终点可能偏移几米。
    //   （真要根治得重跑 `_gen_vehicle_lanes.py` 把这 261 条接上去，本次不动数据。）
    // -----------------------------------------------------------------------------------------
    private static void MarkRoutableLanes(Table table)
    {
        // 迭代版 Tarjan（14171 个点，递归会爆栈）。
        var index = new Dictionary<int, int>();
        var low = new Dictionary<int, int>();
        var onStack = new HashSet<int>();
        var stack = new List<int>();
        var comp = new Dictionary<int, int>();
        var counter = 0;
        var compCount = 0;

        foreach (var root in table.Lanes.Keys)
        {
            if (index.ContainsKey(root))
                continue;

            var work = new List<(int Node, IEnumerator<int> It)> { (root, Next(table, root)) };
            index[root] = low[root] = counter++;
            stack.Add(root);
            onStack.Add(root);

            while (work.Count > 0)
            {
                var (v, it) = work[^1];
                var advanced = false;

                while (it.MoveNext())
                {
                    var w = it.Current;
                    if (!table.Lanes.ContainsKey(w))
                        continue;
                    if (!index.ContainsKey(w))
                    {
                        index[w] = low[w] = counter++;
                        stack.Add(w);
                        onStack.Add(w);
                        work.Add((w, Next(table, w)));
                        advanced = true;
                        break;
                    }
                    if (onStack.Contains(w) && index[w] < low[v])
                        low[v] = index[w];
                }

                if (advanced)
                    continue;

                work.RemoveAt(work.Count - 1);
                if (low[v] == index[v])
                {
                    var id = compCount++;
                    int popped;
                    do
                    {
                        popped = stack[^1];
                        stack.RemoveAt(stack.Count - 1);
                        onStack.Remove(popped);
                        comp[popped] = id;
                    }
                    while (popped != v);
                }

                if (work.Count > 0)
                {
                    var parent = work[^1].Node;
                    if (low[v] < low[parent])
                        low[parent] = low[v];
                }
            }
        }

        // 取规模最大的分量作为「可用路网」。
        var sizes = new Dictionary<int, int>();
        foreach (var c in comp.Values)
            sizes[c] = sizes.TryGetValue(c, out var n) ? n + 1 : 1;

        var best = -1;
        var bestSize = 0;
        foreach (var (id, size) in sizes)
        {
            if (size <= bestSize)
                continue;
            bestSize = size;
            best = id;
        }

        var routable = 0;
        foreach (var (handle, lane) in table.Lanes)
        {
            lane.Routable = comp.TryGetValue(handle, out var c) && c == best;
            if (lane.Routable)
                routable++;
        }

        if (routable == 0)
        {
            // 兜底：一旦上面的分量算挂了（退化成全 false）导航会**彻底不可用**，
            // 宁可退回老行为（全都可用）也不要让自动驾驶直接死掉。
            foreach (var lane in table.Lanes.Values)
                lane.Routable = true;
            routable = table.Lanes.Count;
        }

        table.RoutableLanes = routable;
        table.RoutableComponents = sizes.Count;
    }

    private static IEnumerator<int> Next(Table table, int handle)
        => table.Lanes.TryGetValue(handle, out var lane)
            ? ((IEnumerable<int>)lane.Next).GetEnumerator()
            : Enumerable.Empty<int>().GetEnumerator();

    private static float Num(string s) => float.TryParse(s, out var v) ? v : 0f;

    private static (int, int) CellOf(float x, float z)
        => ((int)MathF.Floor(x / CellSize), (int)MathF.Floor(z / CellSize));

    /// <summary>
    /// 在 (px,pz) 的**环形**区间内随机挑一条车道当作出生点：距玩家
    /// <c>[minDistance, radius]</c>。
    ///
    /// ⛑ 为什么必须有 <c>minDistance</c>（2026-09-29 修）：原来只筛「≤ radius」，
    ///   于是车会直接刷在玩家脚边几米处；而且出生点固定取车道<b>起点</b>（Distance=0），
    ///   而车道起点大量落在路口里 ⇒ 观感就是「车刷在我身边，还不在车道上」。
    ///   更糟的是客户端 <c>AetherVehicleViewManager</c> 会在
    ///   <c>CANDIDATE_GO_DISTANCE_SQ=400</c>（20m）内把车流车提升成 GO（完整载具实体），
    ///   提升后不再吃我们的 Aether 车道插值 ⇒ 冻在原地，于是玩家身边越积越多不动的车。
    /// </summary>
    internal static bool TryPickSpawn(float px, float pz, float radius, float minDistance, Random rng,
        out DriveState state, out float x, out float y, out float z, out float speed)
    {
        state = default;
        x = y = z = 0f;
        speed = 9.72f;

        var table = Cache.Value;
        if (!table.Ok)
            return false;

        var reach = (int)MathF.Ceiling(radius / CellSize);
        var baseCell = CellOf(px, pz);
        List<int> pool = [];
        for (var dx = -reach; dx <= reach; dx++)
        for (var dz = -reach; dz <= reach; dz++)
        {
            if (!table.Cells.TryGetValue((baseCell.Item1 + dx, baseCell.Item2 + dz), out var bucket))
                continue;
            foreach (var h in bucket)
            {
                if (!table.Lanes.TryGetValue(h, out var lane))
                    continue;
                // ⛑ 2026-09-29：不要生在**没有后继**的车道上。
                //   链接表只覆盖 13910/14171，剩下 261 条是无后继的真死路；
                //   生在那种车道上 ⇒ 车开到尽头就被回收，实机表现为「刚刷出来就没了」
                //   （日志里 alive=3~20s、hops=0、after=13~211m 全是这批）。
                if (lane.Next.Length == 0)
                    continue;
                var ddx = lane.Bx - px;
                var ddz = lane.Bz - pz;
                var dSq = ddx * ddx + ddz * ddz;
                if (dSq <= radius * radius && dSq >= minDistance * minDistance)
                    pool.Add(h);
            }
        }
        if (pool.Count == 0)
        {
            // 环太窄（min 太大 / 附近没路）⇒ 退回「只要不在玩家脸上」，宁可近一点也别刷不出来。
            var minSq = minDistance * minDistance;
            for (var dx = -reach; dx <= reach; dx++)
            for (var dz = -reach; dz <= reach; dz++)
            {
                if (!table.Cells.TryGetValue((baseCell.Item1 + dx, baseCell.Item2 + dz), out var bucket))
                    continue;
                foreach (var h in bucket)
                {
                    if (!table.Lanes.TryGetValue(h, out var lane))
                        continue;
                    var ddx = lane.Bx - px;
                    var ddz = lane.Bz - pz;
                    if (ddx * ddx + ddz * ddz <= radius * radius)
                        pool.Add(h);
                }
                if (pool.Count > 0)
                    break;
            }
            if (pool.Count == 0)
                return false;
        }

        var picked = pool[rng.Next(pool.Count)];
        var start = table.Lanes[picked];
        state = new DriveState { Lane = picked, Distance = 0f };
        x = start.Bx;
        y = start.By;
        z = start.Bz;
        speed = start.Speed;
        return true;
    }

    /// <summary>
    /// 「召唤载具从车道开过来」的生成点（2026-10-03）：在 (px,pz) 的<b>环形</b>区间
    /// <c>[minDistance, maxDistance]</c> 内，挑一条车道上距离合适的点，并返回<b>车道朝向</b>。
    ///
    /// ⛑⛑ 与 <see cref="TryPickSpawn"/>（T3-7 车流出生）的三点差别，别混用：
    ///   ① <b>只挑 <see cref="Lane.Routable"/> 的车道</b>。吸附到那 285 条孤岛车道上 ⇒
    ///      Dijkstra 解不出路 ⇒ 一条 Cruise 都不发 ⇒ 「叫了车但没反应」（2026-09-30 血泪）。
    ///   ② <b>不只取车道起点</b>：沿弦采样 5 个 t，用落在环内的那一段 —— 车道起点大量落在路口里，
    ///      刷在路口的车开出来会横穿。
    ///   ③ <b>返回车道朝向</b>（<see cref="FacingOf"/> 同一套角度约定），车头顺着路，
    ///      不然车要先原地掉头，观感是「车卡了一下才走」。
    ///
    /// <paramref name="maxLateralMeters"/>：允许的<b>横向偏离</b>（米），0 = 不过滤（旧行为）。
    ///   车道 CSV 只有弦端点，弯道时弦中点在真实路面之外 ⇒ 车会刷到人行道/花坛里。
    ///   给 1.5 之类的值后，弯车道只保留<b>靠近端点</b>的那些采样点（偏离随 t 两端趋 0）。
    /// <paramref name="rng"/>：多台车连叫时不想每次都刷在同一个位置，交给调用方给随机源。
    /// </summary>
    internal static bool TryPickDriveInSpawn(float px, float pz, float minDistance, float maxDistance,
        float maxLateralMeters, Random rng, out float x, out float y, out float z, out float facingDegrees)
    {
        x = y = z = 0f;
        facingDegrees = 0f;

        var table = Cache.Value;
        if (!table.Ok)
            return false;

        var minSq = minDistance * minDistance;
        var maxSq = maxDistance * maxDistance;
        var reach = (int)MathF.Ceiling(maxDistance / CellSize) + 1;
        var baseCell = CellOf(px, pz);

        List<(float X, float Y, float Z, float Facing)> pool = [];
        for (var dx = -reach; dx <= reach; dx++)
        for (var dz = -reach; dz <= reach; dz++)
        {
            if (!table.Cells.TryGetValue((baseCell.Item1 + dx, baseCell.Item2 + dz), out var bucket))
                continue;

            foreach (var h in bucket)
            {
                if (!table.Lanes.TryGetValue(h, out var lane))
                    continue;
                // ⛑⛑ 孤岛车道（无后继 / 不在最大强连通分量内）一律不要。
                if (!lane.Routable || lane.Next.Length == 0)
                    continue;

                // 角度约定与 SpawnDirectAsync 的偏移一致：
                //   yaw = facing * PI/180；偏移 = (+cos(yaw), -sin(yaw))
                //   ⇒ 方向向量 (dx,dz) 对应 facing = Atan2(-dz, dx)。
                var sdx = lane.Ex - lane.Bx;
                var sdz = lane.Ez - lane.Bz;
                var facing = MathF.Atan2(-sdz, sdx) * (180f / MathF.PI);

                // ⛑⛑ 采样密度从 5 提到 21：加了横向偏离过滤之后，弯车道只剩端点附近可用，
                //   采样太稀会「明明有合格车道却一个点都挑不到」⇒ 白白退回旧路径。
                const int Samples = 20;
                for (var k = 0; k <= Samples; k++)
                {
                    var t = (float)k / Samples;

                    // ⛑⛑ 弦上 t 处偏离真实车道太远 ⇒ 这个点在人行道 / 花坛 / 建筑里，不要。
                    if (maxLateralMeters > 0f && LateralDeviationAt(lane.Sagitta, t) > maxLateralMeters)
                        continue;

                    var cx = lane.Bx + sdx * t;
                    var cy = lane.By + (lane.Ey - lane.By) * t;
                    var cz = lane.Bz + sdz * t;

                    var ddx = cx - px;
                    var ddz = cz - pz;
                    var dSq = ddx * ddx + ddz * ddz;
                    if (dSq < minSq || dSq > maxSq)
                        continue;

                    pool.Add((cx, cy, cz, facing));
                }
            }
        }

        if (pool.Count == 0)
            return false;

        var picked = pool[rng.Next(pool.Count)];
        x = picked.X; y = picked.Y; z = picked.Z; facingDegrees = picked.Facing;
        return true;
    }

    // ---------------------------------------------------------------------------------------------
    // 路线规划（GameRouter.VehicleNav.cs）用的取值口。数据仍然只有这一份：
    // 只为「最近车道」另建了一份索引（按起点/中点/终点三个关键 cell 挂号），因为 try 9 是长车道
    // 只按起点挂号会漏掉「从路中间上车」的情形，而我们不能动 TryPickSpawn 那张 begin-only 索引
    // （改了会改变 T3-7 车流的随机出生分布）。
    // ---------------------------------------------------------------------------------------------

    private sealed class NavIndex
    {
        internal Dictionary<(int, int), List<int>> Cells = [];
    }

    private static readonly Lazy<NavIndex> NavCache = new(BuildNavIndex);

    private static NavIndex BuildNavIndex()
    {
        var index = new NavIndex();
        foreach (var (handle, lane) in Cache.Value.Lanes)
        {
            Index(handle, lane.Bx, lane.Bz);
            Index(handle, (lane.Bx + lane.Ex) * 0.5f, (lane.Bz + lane.Ez) * 0.5f);
            Index(handle, lane.Ex, lane.Ez);
        }

        void Index(int h, float x, float z)
        {
            var cell = CellOf(x, z);
            if (!index.Cells.TryGetValue(cell, out var bucket))
                index.Cells[cell] = bucket = [];
            if (!bucket.Contains(h))
                bucket.Add(h);
        }

        return index;
    }

    /// <summary>一次 <c>TrySnapLane</c> 的结果。</summary>
    internal readonly record struct LaneSnap(int Handle, float T, float Distance, float X, float Y, float Z);

    /// <summary>
    /// 找 (px,pz) 半径内最近的车道，并把点投影到它的弦上（<c>T</c> = 归一化里程）。
    /// 只看 XZ 平面：竖直方向(y) 对「哪条路最近」没有意义，而我们本来也只有端点。
    ///
    /// ⛑⛑ 2026-09-30：<b>只返回 <see cref="Lane.Routable"/> 的车道</b>（最大强连通分量内的）。
    ///   吸附到孤儿车道上 ⇒ Dijkstra 永远解不出路 ⇒ 按自动驾驶没反应。详见 <see cref="MarkRoutableLanes"/>。
    ///   车流的 <c>TryPickSpawn</c> 不受影响（那条路径不走这里）。
    /// </summary>
    internal static bool TrySnapLane(float px, float pz, float maxRadius, out LaneSnap snap)
        => TrySnapLane(px, pz, maxRadius, 0f, out snap);

    /// <summary>
    /// 同 <see cref="TrySnapLane(float, float, float, out LaneSnap)"/>，多一个<b>横向偏离</b>过滤。
    ///
    /// ⛑⛑ 召唤车「停在人行道 / 花坛里」的另一半根因：停车点是玩家坐标投影到最近车道<b>弦</b>上
    /// 得到的，弯车道时这个点同样在路面之外。给 <paramref name="maxLateralMeters"/>（如 1.5）
    /// 即可只接受贴近真实路面的投影位置。
    ///
    /// ⛑ 传 0（= 上面那个重载）保持旧行为，自动驾驶导航走的就是那条，别动它。
    /// </summary>
    internal static bool TrySnapLane(float px, float pz, float maxRadius, float maxLateralMeters,
        out LaneSnap snap)
    {
        snap = default;
        var table = Cache.Value;
        if (!table.Ok)
            return false;

        var reach = (int)MathF.Ceiling(maxRadius / CellSize) + 1;
        var baseCell = CellOf(px, pz);
        var best = float.MaxValue;
        var found = false;

        for (var dx = -reach; dx <= reach; dx++)
        for (var dz = -reach; dz <= reach; dz++)
        {
            if (!NavCache.Value.Cells.TryGetValue((baseCell.Item1 + dx, baseCell.Item2 + dz), out var bucket))
                continue;

            foreach (var h in bucket)
            {
                if (!table.Lanes.TryGetValue(h, out var lane))
                    continue;

                // ⛑⛑ 跳过孤岛车道：它们参与不了任何路径（见 MarkRoutableLanes 的注释）。
                if (!lane.Routable)
                    continue;

                var sx = lane.Ex - lane.Bx;
                var sz = lane.Ez - lane.Bz;
                var denom = sx * sx + sz * sz;
                var t = denom > 1e-6f
                    ? ((px - lane.Bx) * sx + (pz - lane.Bz) * sz) / denom
                    : 0f;
                t = Math.Clamp(t, 0f, 1f);

                // ⛑⛑ 弦上 t 处偏离真实车道太远 ⇒ 停车点会落在人行道 / 花坛里，换一条。
                if (maxLateralMeters > 0f && LateralDeviationAt(lane.Sagitta, t) > maxLateralMeters)
                    continue;

                var cx = lane.Bx + sx * t;
                var cz = lane.Bz + sz * t;
                var ddx = cx - px;
                var ddz = cz - pz;
                var dist = MathF.Sqrt(ddx * ddx + ddz * ddz);
                if (dist > maxRadius || dist >= best)
                    continue;

                best = dist;
                snap = new LaneSnap(h, t, dist, cx, lane.By + (lane.Ey - lane.By) * t, cz);
                found = true;
            }
        }

        return found;
    }

    /// <summary>一条车道的弦与长度。找不到返回 false。</summary>
    internal static bool TryLaneGeometry(int handle, out float bx, out float by, out float bz,
        out float ex, out float ey, out float ez, out float length)
    {
        bx = by = bz = ex = ey = ez = length = 0f;
        if (!Cache.Value.Lanes.TryGetValue(handle, out var lane))
            return false;

        bx = lane.Bx; by = lane.By; bz = lane.Bz;
        ex = lane.Ex; ey = lane.Ey; ez = lane.Ez;
        length = lane.Length;
        return true;
    }

    /// <summary>后继车道（多值）。没有后继返回空数组。</summary>
    internal static int[] NextOf(int handle)
        => Cache.Value.Lanes.TryGetValue(handle, out var lane) ? lane.Next : [];

    /// <summary>
    /// 规划一条**直行优先**的路径：从起始车道出发，每一步都挑「方向最贴合当前朝向」的后继。
    /// 返回接下来要走的车道序列（不含起始车道本身）。
    ///
    /// ★★ 为什么需要它（2026-09-28）：车流原来是每到一条车道就**随机挑后继**（<c>PickNext</c>），
    ///   于是车会乱拐、掉头、钻死路、很快被回收 —— 用户看到的就是「开很短距离就停下/消失」。
    ///   有了预规划路径，车沿道路连续跑 1~2 段甚至更远，观感从"随机乱窜"变成"正常行驶"。
    ///
    /// ★ 不直接用导航那套 Dijkstra：Dijkstra 求的是**到指定目标**的最短路，
    ///   而车流没有目的地，要的是"顺着路往前开"。直行优先恰好表达这个意图，
    ///   且天然排除掉头（dot 为负的后继排最后）。
    ///
    /// ★ 接缝超过 <paramref name="maxHop"/> 的后继直接跳过 —— 与 <c>Advance</c> 的硬上限一致，
    ///   否则规划出来的路径里会混进几百米的瞬移跳。
    /// </summary>
    internal static List<int> PlanRoute(int startLane, int maxHops, float maxHop)
    {
        var route = new List<int>(Math.Max(0, maxHops));
        var table = Cache.Value;
        if (!table.Ok)
            return route;

        var cur = startLane;
        for (var k = 0; k < maxHops; k++)
        {
            if (!table.Lanes.TryGetValue(cur, out var lane))
                break;

            var fdx = lane.Ex - lane.Bx;
            var fdz = lane.Ez - lane.Bz;
            var flen = MathF.Sqrt(fdx * fdx + fdz * fdz);
            if (flen < 1e-3f)
                break;
            fdx /= flen;
            fdz /= flen;

            var best = -1;
            var bestDot = -2f;
            foreach (var h in lane.Next)
            {
                if (!table.Lanes.TryGetValue(h, out var cand))
                    continue;
                var gx = cand.Bx - lane.Ex;
                var gz = cand.Bz - lane.Ez;
                if (gx * gx + gz * gz > maxHop * maxHop)
                    continue;                       // 会瞬移的后继不用
                var cdx = cand.Ex - cand.Bx;
                var cdz = cand.Ez - cand.Bz;
                var clen = MathF.Sqrt(cdx * cdx + cdz * cdz);
                if (clen < 1e-3f)
                    continue;
                var dot = fdx * (cdx / clen) + fdz * (cdz / clen);
                if (dot > bestDot)
                {
                    bestDot = dot;
                    best = h;
                }
            }

            if (best < 0)
                break;                              // 没有合格后继 = 到头了
            route.Add(best);
            cur = best;
        }
        return route;
    }

    /// <summary>一次车道衔接的细节（诊断用）。</summary>
    internal struct LaneHop
    {
        internal int FromLane;
        internal int ToLane;

        /// <summary>旧车道终点 → 新车道起点的接缝距离（米）。越大越像瞬移。</summary>
        internal float Gap;
    }

    /// <summary>
    /// 沿车道推进指定距离（米）。换道时把 <paramref name="state"/> 切到下一条并把里程归零。
    /// 返回 false = 走到路网死路，调用方应当回收这辆车。
    ///
    /// ★ <paramref name="maxGap"/>：只在接缝 ≤ 该值的后继里挑。捐赠端 next 是多值后继，
    ///   实测 5.4% 的后继接缝 &gt; 12m（最大 340m），直接随机挑会让车「瞬移一段甚至倒退」。
    ///   找不到合格后继时退回「接缝最小」的那条，并如实把 Gap 报给调用方。
    /// </summary>
    /// <param name="maxHop">
    /// 换道接缝的**绝对硬上限**（米）。超过就判定"开到尽头"，返回 false 让调用方回收。
    /// 为什么需要它：<c>PickNext</c> 在所有后继都被"方向反折"过滤掉时会退回 <c>fallback</c>
    /// （全局最小接缝）—— 那条可能远在天边。实测日志里出现过 <c>worstGap=394.1m</c>，
    /// 而车道数据本身 p99 接缝就有 347m、max 2965m（>30m 的占 13.92%）。
    /// 跳过去就是几百米的瞬移，比车在路口消失难看得多 —— 宁可回收。
    /// </param>
    internal static bool Advance(ref DriveState s, float meters, Random rng, float maxGap,
        out LaneHop? hop, float maxHop = float.MaxValue, int preferredNext = -1)
    {
        hop = null;
        var table = Cache.Value;
        if (!table.Ok)
            return false;

        var guard = 0;
        while (guard++ < 32)
        {
            if (!table.Lanes.TryGetValue(s.Lane, out var lane))
                return false;

            var remain = lane.Length - s.Distance;
            if (meters < remain)
            {
                s.Distance += meters;
                return true;
            }

            if (lane.Next.Length == 0)
            {
                s.Distance = lane.Length;
                return false;
            }

            meters -= remain;
            var from = s.Lane;

            // ★ 有预规划路径且这一跳就用它；否则退回「最小接缝 + 不反折」的随机挑法。
            int next;
            float gap;
            if (preferredNext >= 0 && Array.IndexOf(lane.Next, preferredNext) >= 0)
            {
                next = preferredNext;
                gap = 0f;
                if (table.Lanes.TryGetValue(next, out var cand))
                {
                    var ax = cand.Bx - lane.Ex;
                    var az = cand.Bz - lane.Ez;
                    gap = MathF.Sqrt(ax * ax + az * az);
                }
            }
            else
            {
                next = PickNext(lane, table, rng, maxGap, out gap);
            }

            if (next < 0 || gap > maxHop)
            {
                // 接缝太远 ⇒ 不跳，当作开到尽头，交给调用方回收。
                // 硬跳会造成数百米的瞬移（见 maxHop 的注释）。
                s.Distance = lane.Length;
                return false;
            }
            s.Lane = next;
            s.Distance = 0f;
            hop = new LaneHop { FromLane = from, ToLane = next, Gap = gap };
        }
        return true;
    }

    /// <summary>
    /// 挑下一条车道：在「接缝最小」的基础上放宽 <paramref name="maxGap"/> 米的容差再随机选，
    /// 并排除方向反折的后继（那会让车掉头，视觉上像倒退瞬移）。
    ///
    /// ★ 为什么是**相对容差**而不是「接缝 ≤ N 米」的绝对阈值：
    ///   实测 400 起点 × 600 跳，绝对阈值 8m 会让 15.05% 的衔接 &gt; 12m、p99 达 607m；
    ///   而「总选最小缝」只有 2.55% &gt; 12m、p99 50m。原因是绝对阈值一旦命中就随机选，
    ///   容易拐进后继质量差的车道；贪心取最近则倾向留在连贯的主干道上。
    ///   所以 maxGap=0 = 严格取接缝最小（并列最小里随机），是实测最优。
    /// </summary>
    private static int PickNext(Lane from, Table table, Random rng, float maxGap, out float gap)
    {
        var fex = from.Ex;
        var fez = from.Ez;
        var fdx = fex - from.Bx;
        var fdz = fez - from.Bz;

        List<int> good = [];
        List<int> forward = [];
        var minGap = float.MaxValue;
        var fallback = from.Next[0];

        foreach (var h in from.Next)
        {
            if (!table.Lanes.TryGetValue(h, out var cand))
                continue;

            var vx = cand.Bx - fex;
            var vz = cand.Bz - fez;
            var d = MathF.Sqrt(vx * vx + vz * vz);
            if (d < minGap)
            {
                minGap = d;
                fallback = h;
            }

            // 方向反折：新车道朝向与旧车道相反 → 车会「掉头」，视觉上像倒退瞬移。
            if (fdx * (cand.Ex - cand.Bx) + fdz * (cand.Ez - cand.Bz) < 0f)
                continue;

            forward.Add(h);
            if (d <= minGap + maxGap)
                good.Add(h);
        }

        gap = minGap;
        if (good.Count > 0)
            return good[rng.Next(good.Count)];
        if (forward.Count > 0 && maxGap > 0f)
            return forward[rng.Next(forward.Count)];
        return fallback;
    }

    /// <summary>服务端侧的线性插值的粗略世界坐标（只用于 AOI 判定 / 日志，客户端自己有真实曲线）。</summary>
    internal static void PositionOf(in DriveState s, out float x, out float y, out float z)
    {
        var table = Cache.Value;
        if (!table.Lanes.TryGetValue(s.Lane, out var lane))
        {
            x = y = z = 0f;
            return;
        }
        var t = Math.Clamp(s.Distance / lane.Length, 0f, 1f);
        x = lane.Bx + (lane.Ex - lane.Bx) * t;
        y = lane.By + (lane.Ey - lane.By) * t;
        z = lane.Bz + (lane.Ez - lane.Bz) * t;
    }

    /// <summary>距当前车道末端还有多少米（用于「接近路口就减速」）。</summary>
    internal static float RemainOf(in DriveState s)
    {
        var table = Cache.Value;
        if (!table.Lanes.TryGetValue(s.Lane, out var lane))
            return 0f;
        return MathF.Max(0f, lane.Length - s.Distance);
    }

    /// <summary>当前车道的限速（m/s）。车换道后会变，所以每 tick 都要重取。</summary>
    internal static float LimitOf(in DriveState s)
    {
        var table = Cache.Value;
        return table.Lanes.TryGetValue(s.Lane, out var lane) ? lane.Speed : 9.72f;
    }

    /// <summary>
    /// 朝向 <b>弧度</b>（0 = +Z，顺时针为正），由车道端点方向推出。
    /// ★ 2026-09-21 修正：原先这里返回「度」却被直接填进 ClientVehicleInitData.Facing，
    ///   实测线上包里 Facing = -88.5 —— 那是度数当弧度用了。Facing 是弧度，
    ///   EulerAngles 才是度（调用处各自换算）。
    /// </summary>
    internal static float FacingOf(in DriveState s)
    {
        var table = Cache.Value;
        if (!table.Lanes.TryGetValue(s.Lane, out var lane))
            return 0f;
        var dx = lane.Ex - lane.Bx;
        var dz = lane.Ez - lane.Bz;
        if (MathF.Abs(dx) < 1e-4f && MathF.Abs(dz) < 1e-4f)
            return 0f;
        // ★ 单位 = 度，与已生效的 SyncAetherAICrowdAdd 一致（那里 Facing 与 EulerAngles.y
        //   填的是同一个值）。曾误改为弧度，导致下发 -88.5 被当成弧度（-5070°）。
        return MathF.Atan2(dx, dz) * 180f / MathF.PI;
    }
}