Tutorial/MultiWheel/MultiWheelC/PilotDefinition.cs

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Net.Http;
using System.Numerics;
using System.Threading;
using System.Threading.Tasks;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using ClumsyCore.Utilities;
using CommonUsage;
using CommonUsage.Chassis;
using FundamentalLib;
using FundamentalLib.Utilities;
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
using Newtonsoft.Json;
using Vector = ClumsyCore.Utilities.Vector;

namespace MultiWheelC;

public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefinition>
{
    public new float CarLength = 1472f;
    public new float CarWidth = 948f;

    public override void StandardInit()
    {
        InitMultiVehicleCoordination();
    }

    #region MultiVehicleCoordination (从 MDCSToolBox 内联回 Tutorial)

    [AsLowerIO(desc = "启用（手动）多车联动")] public bool MultiVehicleManualEnabled;
    [AsLowerIO(desc = "（手动）多车联动模式")] public int MultiVehicleManualMode;
    [FieldMember(desc = "多车联动已同步")] public bool MultiVehicleAligned;

    [AsLowerIO(desc = "多车联动：遥控器Vx")] public float MultiVehicleManualVx;
    [AsLowerIO(desc = "多车联动：遥控器Vy")] public float MultiVehicleManualVy;
    [AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth;

    // ===== Clumsy 侧脚本/动作驱动的手动等价输入（不走 Medulla IO，不会被 IO 同步覆盖）=====
    // 手动 IO 字段是 [AsLowerIO]（Medulla→Clumsy，Medulla 每周期回写），Clumsy 侧 MovementTest 写它们会被覆盖。
    // 因此提供这组内部字段，让 Clumsy 侧动作（如 FleetRotateInPlace）能像 FleetRemote 一样驱动车队联动：
    // ScriptEnabled=使能；Mode 0=常规 1=蟹行 2=原地旋转；Vx/Vy/Vth 语义与手动遥控完全一致（m/s、m/s、deg/s）。
    [FieldMember(desc = "多车联动：脚本驱动使能")] public bool MultiVehicleScriptEnabled;
    [FieldMember(desc = "多车联动：脚本驱动模式")] public int MultiVehicleScriptMode;
    [FieldMember(desc = "多车联动：脚本驱动Vx")] public float MultiVehicleScriptVx;
    [FieldMember(desc = "多车联动：脚本驱动Vy")] public float MultiVehicleScriptVy;
    [FieldMember(desc = "多车联动：脚本驱动Vth")] public float MultiVehicleScriptVth;

    [AsUpperIO(desc = "多车联动：自动驾驶", timeOutReset = true)] public bool MultiVehicleAutoEnabled;
    [FieldMember(desc = "多车联动：自动驾驶Vx")] public float MultiVehicleAutoVx;
    [FieldMember(desc = "多车联动：自动驾驶FrontTh")] public float MultiVehicleAutoFrontTh;
    [FieldMember(desc = "多车联动：自动驾驶RearTh")] public float MultiVehicleAutoRearTh;

    public Dictionary<int, VehicleSyncInfo> MultiVehicleFleet = new();
    public VehicleSyncNotification MultiVehicleNotification;

    // A: 互斥锁固定为独立 readonly 对象，绝不随 MultiVehicleFleet 字段被整体替换而失效。
    //    所有对 MultiVehicleFleet / _multiVehicleFleetSeen 的读写都必须 lock(FleetLock)。
    public readonly object FleetLock = new();

    // C: 各 fleet 成员最近一次被 register/notify 刷新的本地时刻（用本地时钟，避免远端时钟偏差）。
    //    主车 Tick 据此剔除掉线成员；编队就绪要求所有成员新鲜。
    private readonly Dictionary<int, DateTime> _multiVehicleFleetSeen = new();

    [FieldMember(desc = "多车联动：车队姿态x")] public float CenterX;
    [FieldMember(desc = "多车联动：车队姿态y")] public float CenterY;
    [FieldMember(desc = "多车联动：车队姿态th")] public float CenterTh;

    // G: 车队中心原子快照。三个 float 无法整体原子写，改为整体新建对象后引用赋值（引用赋值原子），
    //    路径控制器线程只读最近一次完整快照，避免读到 x 新 / y,th 旧的撕裂组合。
    public sealed class FleetCenterSnapshot
    {
        public float X, Y, Th;
        public long Tick;
    }
    private volatile FleetCenterSnapshot _fleetCenter = new();
    public FleetCenterSnapshot GetFleetCenterSnapshot() => _fleetCenter;
    private void PublishFleetCenter(float x, float y, float th)
    {
        CenterX = x;
        CenterY = y;
        CenterTh = th;
        _fleetCenter = new FleetCenterSnapshot { X = x, Y = y, Th = th, Tick = DateTime.Now.Ticks };
    }

    // B: 路径控制器最近一次写入自动速度命令的时刻；超时即视为失效（路径结束/早退/卡顿），清零下发。
    public DateTime MultiVehicleAutoCmdTime = DateTime.MinValue;
    // D: 路径控制器透传的理想车队中心位姿（世界系），由 idealPos/idealAngle 写入。
    public bool MultiVehicleAutoHasIdeal;
    public float MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh;

    // F: notify 序列号（主车单调递增）与从车已应用的最大序列号（丢弃乱序旧包）。
    private long _multiVehicleNotifySeq;
    private long _multiVehicleAppliedSeq = -1;

    [AsLowerIO(desc = "车号")] public int CarNum = 1;

    [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
    [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
    [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
    [AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
    [AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
    [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
    [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
    [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;

    [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
    [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;

    private float _multiVehicleAccumulateTh;
    private DateTime _multiVehicleLastThTime = DateTime.Now;
    private readonly object _multiVehicleNotificationLock = new();
    private DateTime _multiVehicleLastNotifyTime = DateTime.MinValue;
    private bool _multiVehicleSyncInitialized;

    // 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
    private DateTime _mvDbgLastLog = DateTime.MinValue;
    private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist;
    // 原地旋转(mode2)实际叠加的车体系纠偏旋量(mm/s, mm/s, deg/s)，仅用于诊断日志。
    private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
    // 原地旋转纠偏 PI 控制器的积分累加器(mm·s, mm·s, deg·s)与上次计算时刻。
    private float _rotIntegX, _rotIntegY, _rotIntegTh;
    private float _rotOmegaPeak; // 本次旋转过程中观测到的指令角速度峰值(deg/s)，用于纠偏随转速缩放
    private DateTime _rotPiLastTime = DateTime.MinValue;

    // 原地旋转“两车实际 sim 位姿”诊断采样状态（仅主车，通过 Playground WebAPI 读取）。
    private DateTime _rotPoseLastLog = DateTime.MinValue;
    private DateTime _rotPosePrevTime = DateTime.MinValue;
    private float _rotPosePrevYaw1, _rotPosePrevYaw2;
    private bool _rotPoseEpisode;          // 本次旋转片段是否已捕获参考量
    private float _rotPoseMid0X, _rotPoseMid0Y; // 起转瞬间车队几何中心(世界系)，用于度量整体平移漂移

    // 写盘诊断日志（Clumsy 侧）。改用 DLog 落盘：同一 topic 的日志会归到以 topic 命名的文件夹，
    // 由 DLog 统一管理落点/滚动，无需自行维护文件句柄与 session 清空逻辑。
    private DateTime _mvDiskLastLog = DateTime.MinValue;
    private void FleetDiag(string msg)
    {
        DLog.Log($"car{CarNum} {msg}", "FleetDiagClumsy");
    }

    // 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
    private readonly object _neighborDetectLock = new();
    private readonly List<(DateTime Time, Vector2 Src, Vector2 Dst)> _neighborDetects = new();

    // 互识别：上一帧检测中心，用作下一帧猜测（闭环跟踪），与「多舵轮-2腿检测」MovementTest 一致
    private Vector2 _neighborGuess;
    private bool _neighborGuessValid;
    private DateTime _neighborGuessTime = DateTime.MinValue;

    /// <summary>
    /// 互识别钩子：用单线雷达识别邻车的两腿/两轮，换算成本车体坐标系下相对“理应对齐参考点”的偏差。
    /// 全对齐时返回 (true, 0, 0, 0)；未检测到时返回 (false, ...)。
    /// detectDistance = 编队车间距 - 检测中心偏移（即邻车检测中心到本车原点的标称距离，恒为正值，方向由 TwoLegGuessX 符号决定）。
    /// </summary>
    private (bool Valid, float Dx, float Dy, float Dth, float NeighborDist) DetectNeighborBias(float detectDistance)
    {
        // 检测猜测点与「多舵轮-2腿检测」MovementTest 完全一致：首帧 / 跟丢回落都用 Conf.TwoLegGuessX
        // （方向与距离都由它一处决定），跟到后用上一帧中心闭环跟踪。
        // detectDistance 只用于计算编队对齐偏差/间距，不参与“去哪里找邻车”，避免猜测点偏离导致 ROI 框漏掉真实邻车。
        var nominal = new Vector2(Conf.TwoLegGuessX, 0f);

        var now = DateTime.Now;
        // 闭环跟踪：1s 内检到过就用上一帧中心作猜测，使绿色 ROI 框收敛到真实邻车（与 MovementTest 行为一致）；
        // 跟丢超时则回落到编队标称位置，避免框漂走。
        var guess = (_neighborGuessValid && (now - _neighborGuessTime).TotalSeconds < 1) ? _neighborGuess : nominal;
        var seg = TwoLegDetect.Detect(Conf.TwoLegLidarName, guess.X, TwoLegDetect.SetFilters(guess.X, guess.Y));

        Vector2 avgSrc, avgDst;
        lock (_neighborDetectLock)
        {
            if (seg != null)
            {
                _neighborDetects.Add((now, seg.Src, seg.Dst));
                _neighborGuess = (seg.Src + seg.Dst) / 2f;
                _neighborGuessValid = true;
                _neighborGuessTime = now;
            }
            _neighborDetects.RemoveAll(r => (now - r.Time).TotalSeconds > 1);

            var cnt = _neighborDetects.Count;
            if (cnt == 0)
            {
                _neighborGuessValid = false;
                Hedingben.ToastText($"邻车未检到 (guess x:{guess.X:F0} y:{guess.Y:F0})", $"MultiVehicle{CarNum}-detect");
                return (false, 0f, 0f, 0f, 0f);
            }
            if (cnt > 10) _neighborDetects.RemoveRange(0, cnt - 10);

            avgSrc = new Vector2(_neighborDetects.Average(r => r.Src.X), _neighborDetects.Average(r => r.Src.Y));
            avgDst = new Vector2(_neighborDetects.Average(r => r.Dst.X), _neighborDetects.Average(r => r.Dst.Y));
        }

        // 两腿连线中点为邻车检测中心；连线的垂线方向即邻车朝向
        var dirVec = avgDst - avgSrc;
        var center = (avgSrc + avgDst) / 2f;
        var dir = (float)LessMath.RoundTh(Math.Atan2(dirVec.Y, dirVec.X) / Math.PI * 180 + 90);

        // 从检测中心沿邻车朝向外推 detectDistance，得到“本车理应所在的参考点”（车体系）
        var target = LessMath.Transform2D(center, dir, detectDistance, 0);

        var dx = target.X;
        var dy = target.Y;
        var dth = (float)LessMath.ThDiff(dir, 0);
        var neighborDist = center.Length();

        _mvLastDetCenterX = center.X;
        _mvLastDetCenterY = center.Y;
        _mvLastDetDir = dir;
        _mvLastDetDist = neighborDist;

        var painter = UI.GetPainter("MultiVehicleDetectBias", false);
        painter.Clear();
        painter.DrawLine(Color.DeepPink, center, target, width: 2);
        painter.DrawText(Color.Yellow, $"dx:{dx:F0} dy:{dy:F0} dth:{dth:F2}", center.X, center.Y);

        Hedingben.ToastText(
            $"[联动检测] lidar:{Conf.TwoLegLidarName} guess x:{guess.X:F0} y:{guess.Y:F0} | " +
            $"中心 x:{center.X:F0} y:{center.Y:F0} dir:{dir:F1} dist:{neighborDist:F0} | 偏差 dx:{dx:F0} dy:{dy:F0} dth:{dth:F2}",
            $"MultiVehicle{CarNum}-detect");

        return (true, dx, dy, dth, neighborDist);
    }

    private void InitMultiVehicleCoordination()
    {
        if (_multiVehicleSyncInitialized) return;
        _multiVehicleSyncInitialized = true;

        var hc = new HttpClient { Timeout = TimeSpan.FromSeconds(2) };

        PicoHttpServer.AddGetHandler("/multi-vehicle-register", new { CarNum = 0, Info = "" }, query =>
        {
            try
            {
                var infoJson = Uri.UnescapeDataString(query.Info ?? "");
                var info = JsonConvert.DeserializeObject<VehicleSyncInfo>(infoJson)
                           ?? throw new Exception("VehicleSyncInfo is null");
                lock (FleetLock)
                {
                    MultiVehicleFleet[query.CarNum] = info;
                    _multiVehicleFleetSeen[query.CarNum] = DateTime.Now; // C: 刷新存活时刻
                }
                return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
            }
            catch (Exception e)
            {
                DLog.Log($"/multi-vehicle-register error: {e.FormatEx()}", "MultiVehicle");
                return JsonConvert.SerializeObject(new { code = 500, message = e.Message });
            }
        });

        // F: notify 改用 POST + JSON body（取代 GET query 串），避免整队 Fleet 字典撑爆 URL 长度上限；
        //    用 Seq 丢弃乱序到达的旧包，避免从车短暂套用过期指令。
        PicoHttpServer.AddPostTextHandler("/multi-vehicle-notify", body =>
        {
            try
            {
                var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(body ?? "")
                                   ?? throw new Exception("notification is null");
                // 乱序丢弃：仅应用序列号大于已应用值的包。Seq==0 视为旧版无序列号始终接受；
                // 若 Seq 明显回退(差值>100)，判定为主车重启的新会话，重新接受并对齐序列号。
                if (notification.Seq != 0 && notification.Seq <= _multiVehicleAppliedSeq &&
                    notification.Seq > _multiVehicleAppliedSeq - 100)
                    return JsonConvert.SerializeObject(new { code = 200, message = "stale" });
                _multiVehicleAppliedSeq = notification.Seq;

                MultiVehicleAligned = notification.Aligned;
                lock (FleetLock)
                {
                    MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
                    // C: notify 内含整队成员，逐一刷新其本地存活时刻。
                    var nowSeen = DateTime.Now;
                    foreach (var key in MultiVehicleFleet.Keys)
                        _multiVehicleFleetSeen[key] = nowSeen;
                }
                lock (_multiVehicleNotificationLock)
                {
                    MultiVehicleNotification = notification;
                    _multiVehicleLastNotifyTime = DateTime.Now;
                }
                return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
            }
            catch (Exception e)
            {
                DLog.Log($"/multi-vehicle-notify error: {e.FormatEx()}", "MultiVehicle");
                return JsonConvert.SerializeObject(new { code = 500, message = e.Message });
            }
        });

        DLog.Log("MultiVehicle coordination initialized, starting loop thread", "MultiVehicle");
        Console.WriteLine("[MultiVehicle] coordination initialized, starting loop thread");
        new Thread(() => MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start();
    }

    private void MultiVehicleLoop(HttpClient hc)
    {
        var carLength = CarLength;
        var carWidth = CarWidth;
        var contour = new List<Vector2>
        {
            new(carLength / 2f, carWidth / 2f),
            new(-carLength / 2f, carWidth / 2f),
            new(-carLength / 2f, -carWidth / 2f),
            new(carLength / 2f, -carWidth / 2f),
        };
        var chassis = (MultiWheelChassis)Chassis;
        var sendMotionPainter = UI.GetPainter("MultiWheelChassis-SendMotionVis", false);
        chassis.SendMotionVisualizer = new Visualizer
        {
            LineAction = (color, src, dst, startArrow, endArrow, width) =>
                sendMotionPainter.DrawLine(color, src, dst, startArrow, endArrow, width),
            TextAction = (color, str, pos) => sendMotionPainter.DrawText(color, str, pos),
            Clear = () => sendMotionPainter.Clear()
        };

        DLog.Log("MultiVehicleLoop thread running", "MultiVehicle");
        Console.WriteLine("[MultiVehicle] loop thread running");
        var loopCount = 0L;
        while (true)
        {
            try
            {
                TickMultiVehicle(hc, chassis, contour, sendMotionPainter);
            }
            catch (Exception e)
            {
                DLog.Log($"MultiVehicle loop error: {e.FormatEx()}", "MultiVehicle");
            }

            // 每 ~5s 打一条心跳，确认循环确实在跑（用于区分“循环没跑”与“写盘失败”）
            if (loopCount++ % Math.Max(1, 5000 / Math.Max(1, Conf.MultiVehicleSyncInterval)) == 0)
                DLog.Log($"MultiVehicleLoop heartbeat #{loopCount}", "MultiVehicle");

            Thread.Sleep(Conf.MultiVehicleSyncInterval);
        }
    }

    private void TickMultiVehicle(HttpClient hc, MultiWheelChassis chassis, List<Vector2> contour, Painter sendMotionPainter)
    {
        var isMaster = IsMultiVehicleMaster();
        var autoEnabled = false;
        // 手动等价输入 = Medulla 遥控(IO) 或 Clumsy 脚本/动作驱动（二者择一，脚本优先）。
        var scriptOn = MultiVehicleScriptEnabled;
        var manualEnabled = MultiVehicleManualEnabled || scriptOn;
        // 主车实际生效的手动模式与速度：脚本使能时取脚本字段，否则取 Medulla 手动 IO 字段。
        var manualMode = scriptOn ? MultiVehicleScriptMode : MultiVehicleManualMode;
        var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
        var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
        var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;

        if (isMaster)
            autoEnabled = MultiVehicleAutoEnabled;
        else
        {
            // 从车：自动/手动联动均可由主车广播解锁（无需各自再拨开关）。
            // 仅在 notification 新鲜时认账，主车停发后超时即自动停车，避免用旧指令跑飞。
            lock (_multiVehicleNotificationLock)
            {
                var fresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
                            Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
                if (fresh && MultiVehicleNotification != null)
                {
                    autoEnabled = MultiVehicleNotification.AutoEnabled;
                    manualEnabled = manualEnabled || MultiVehicleNotification.ManualEnabled;
                }
            }
        }

        // 入口诊断（节流 ~300ms）：记录从 Medulla 收到的原始 IO 值与门控判定，
        // 用于确认遥控指令是否真的传到了 Clumsy，以及为何提前 return。
        if ((DateTime.Now - _mvDiskLastLog).TotalMilliseconds >= 300)
        {
            _mvDiskLastLog = DateTime.Now;
            int fleetCnt;
            lock (FleetLock) fleetCnt = MultiVehicleFleet.Count;
            var notifFresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
                             Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
            FleetDiag(
                $"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " +
                $"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
                $"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
                $"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
                $"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
                $"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} useDetour={Conf.MultiVehicleSyncUseDetour}");
        }

        if (!manualEnabled && !autoEnabled)
        {
            UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
            sendMotionPainter.Clear();
            lock (FleetLock)
            {
                MultiVehicleFleet.Clear();
                _multiVehicleFleetSeen.Clear();
            }
            MultiVehicleAutoEnabled = false;
            MultiVehicleAutoHasIdeal = false;
            _multiVehicleAppliedSeq = -1;
            _multiVehicleAccumulateTh = 0f;
            _multiVehicleLastThTime = DateTime.Now;

            if (!isMaster)
                FireAndForgetRegister(hc, BuildSelfInfo(false, false, 0, 0, 0, 0, 0, 0, false));

            return;
        }

        // 自动模式整队姿态依赖 Detour 全局定位（与 MultiVehicleSyncUseDetour 无关）：要求编队至少一台车有定位。
        if (isMaster && autoEnabled && !manualEnabled && !FleetHasPosAvailable())
        {
            MultiVehicleAutoEnabled = false;
            Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
            return;
        }

        // 两类用途解耦（关键语义）：
        //  - useDetourCorrection (= MultiVehicleSyncUseDetour)：是否用 SLAM 位姿做"车队内姿态纠正"(POS 补偿)。
        //    false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
        //  - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
        //    SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
        //    纠偏开启时本车也读。读取若因无有效定位阻塞，则联动线程随之阻塞、不下发速度（安全停车）。
        var autoMode = autoEnabled && !manualEnabled;
        var useDetourCorrection = Conf.MultiVehicleSyncUseDetour;
        var slamRead = useDetourCorrection || (isMaster && autoMode);
        float selfX = 0, selfY = 0, selfTh = 0;
        if (slamRead)
        {
            var carPos = DetourInterface.getCartLocation();
            selfX = (float)carPos.x;
            selfY = (float)carPos.y;
            selfTh = (float)carPos.th;
        }
        // fleetPosValid：整队全局姿态是否已知。主车=自身读到 SLAM；从车=主车广播标志（下方覆盖）。
        var fleetPosValid = slamRead;

        var syncTh = Conf.TestCarSyncTh;
        var syncDistance = Conf.TestCarSyncDistance;
        var deltaDetectCenter = Conf.DeltaDetectCenter;
        float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0, fleetOmega = 0;
        var fleetMode = 0; // 0=常规 1=蟹行 2=原地旋转
        float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
        float crabSteerLimit = 0;
        var crabReverseEquivalent = false;

        if (isMaster)
        {
            if (manualEnabled)
            {
                syncTh = 0;
                fleetMode = manualMode;
                if (fleetMode == 2)
                {
                    // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
                    fleetOmega = manualVth * Conf.ManualCarSyncVthFac;
                    fleetVx = 0;
                    fleetFrontTh = 0;
                    fleetRearTh = 0;
                    _multiVehicleAccumulateTh = 0f;
                    _multiVehicleLastThTime = DateTime.Now;
                }
                else if (fleetMode == 1)
                {
                    // 蟹行：把 (前后向Vx, 横向Vy) 合成速度矢量，四轮同向打到该方向（前后舵轮角相同）。
                    // 舵轮物理/仿真限制约为 ±120°，不要把 ±90° 当边界；否则纯横移附近会在
                    // -90° 与 +90°/反向速度两种等价表示之间跳变。只有超过蟹行舵角上限时才取等价反向。
                    var vx = manualVx * Conf.ManualCarSyncVxFac;
                    var vy = manualVy * Conf.ManualCarSyncVyFac;
                    var speed = (float)Math.Sqrt(vx * vx + vy * vy);
                    var crabAngle = (float)(Math.Atan2(vy, vx) * 180.0 / Math.PI);
                    crabInputVx = vx;
                    crabInputVy = vy;
                    crabRawAngle = crabAngle;
                    crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));

                    if (crabAngle > crabSteerLimit) { crabAngle -= 180f; speed = -speed; crabReverseEquivalent = true; }
                    else if (crabAngle < -crabSteerLimit) { crabAngle += 180f; speed = -speed; crabReverseEquivalent = true; }
                    fleetVx = speed;
                    fleetFrontTh = crabAngle;
                    fleetRearTh = crabAngle;
                    _multiVehicleAccumulateTh = 0f;
                    _multiVehicleLastThTime = DateTime.Now;
                }
                else
                {
                    fleetVx = manualVx * Conf.ManualCarSyncVxFac;
                    var targetTh = manualVth * Conf.ManualCarSyncVthFac;
                    var now = DateTime.Now;
                    var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
                    _multiVehicleLastThTime = now;
                    var sign = Math.Sign(targetTh - _multiVehicleAccumulateTh);
                    _multiVehicleAccumulateTh += sign * Math.Min(Math.Abs(targetTh - _multiVehicleAccumulateTh),
                        Conf.SyncThAccPerSec * dt);
                    fleetFrontTh = _multiVehicleAccumulateTh;
                    fleetRearTh = -fleetFrontTh;
                }
            }
            else
            {
                // B: 自动模式速度命令新鲜度门控。路径结束/回调早退/卡顿后，控制器不再刷新 AutoCmdTime，
                //    超时即视为失效：清零下发速度与 idealPos，关闭 AutoEnabled，避免车队按末速度滑行。
                var autoTimeoutMs = Conf.MultiVehicleAutoCmdTimeoutMs > 0
                    ? Conf.MultiVehicleAutoCmdTimeoutMs
                    : Math.Max(200, Conf.MultiVehicleSyncInterval * 4);
                var autoFresh = (DateTime.Now - MultiVehicleAutoCmdTime).TotalMilliseconds < autoTimeoutMs;
                if (autoFresh)
                {
                    fleetVx = MultiVehicleAutoVx;
                    fleetFrontTh = MultiVehicleAutoFrontTh;
                    fleetRearTh = MultiVehicleAutoRearTh;
                }
                else
                {
                    fleetVx = fleetFrontTh = fleetRearTh = 0;
                    MultiVehicleAutoVx = MultiVehicleAutoFrontTh = MultiVehicleAutoRearTh = 0;
                    MultiVehicleAutoHasIdeal = false;
                    MultiVehicleAutoEnabled = false;
                    Hedingben.ToastText("自动速度命令超时，已停车（路径结束/控制器停发）", "MultiVehicle-auto-timeout");
                }
            }
        }
        else if (MultiVehicleNotification != null)
        {
            VehicleSyncNotification notification;
            lock (_multiVehicleNotificationLock)
                notification = MultiVehicleNotification;

            syncTh = notification.SyncTh;
            syncDistance = notification.SyncDistance;
            deltaDetectCenter = notification.DeltaDetectCenter;
            PublishFleetCenter(notification.CenterX, notification.CenterY, notification.CenterTh);
            // 从车整队姿态来自主车广播：以广播标志作为 fleetPosValid（自身 slamRead 仅决定是否做本车纠偏）。
            fleetPosValid = notification.PosAvailable;
            MultiVehicleAutoEnabled = notification.AutoEnabled;
            fleetVx = notification.FleetVx;
            fleetFrontTh = notification.FleetFrontTh;
            fleetRearTh = notification.FleetRearTh;
            fleetMode = notification.Mode;
            fleetOmega = notification.FleetOmega;
            // D: 从车采用主车广播的理想车队中心（弧线时由 idealPos/idealAngle 而来）做前馈目标。
            MultiVehicleAutoHasIdeal = notification.HasIdeal;
            MultiVehicleAutoIdealX = notification.IdealX;
            MultiVehicleAutoIdealY = notification.IdealY;
            MultiVehicleAutoIdealTh = notification.IdealTh;
        }

        var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);

        if (isMaster)
        {
            lock (FleetLock)
            {
                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
                    layoutX, layoutY, layoutTh, true);
                _multiVehicleFleetSeen[CarNum] = DateTime.Now; // 主车自身恒新鲜
                PruneStaleFleetMembers();                      // C: 剔除掉线从车
            }

            if (TryInferFleetCenter(out var cx, out var cy, out var cth))
                PublishFleetCenter(cx, cy, cth);

            // D: 自动模式（非手动）下，若控制器给出理想车队中心，则以理想位姿作为各车 layout 目标，
            //    使弧线路径上从车按各自相对曲率中心位置前馈，而非仅靠事后检测/SLAM 纠偏。
            if (!manualEnabled && MultiVehicleAutoEnabled &&
                Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal)
                PublishFleetCenter(MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh);
        }

        var selfAligned = !Conf.MultiVehicleUseDetect;
        var detectValid = false;
        float detectDx = 0, detectDy = 0, detectDth = 0;
        var currentSpacing = float.NaN;
        if (Conf.MultiVehicleUseDetect)
        {
            var (valid, dx, dy, dth, neighborDist) = DetectNeighborBias(syncDistance - deltaDetectCenter);
            detectValid = valid;
            if (valid)
            {
                detectDx = dx;
                detectDy = dy;
                detectDth = dth;
                // 检测中心到本车原点距离 + 检测中心偏移 = 两车参考点当前间距
                currentSpacing = neighborDist + deltaDetectCenter;
                selfAligned = Math.Abs(dx) < Conf.SingleCarSyncPrecisionXy &&
                              Math.Abs(dy) < Conf.SingleCarSyncPrecisionXy &&
                              Math.Abs(dth) < Conf.SingleCarSyncPrecisionTh;
            }
            else selfAligned = false;
        }

        // 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（需本车已读到全局定位）
        if (float.IsNaN(currentSpacing) && slamRead)
            currentSpacing = TryGetSlamSpacing(selfX, selfY);

        if (float.IsNaN(currentSpacing))
            Hedingben.ToastText($"间距 当前:N/A 目标:{syncDistance:F0}mm", $"MultiVehicle{CarNum}-spacing");
        else
            Hedingben.ToastText(
                $"间距 当前:{currentSpacing:F0}mm 目标:{syncDistance:F0}mm 差:{currentSpacing - syncDistance:F0}mm",
                $"MultiVehicle{CarNum}-spacing");

        lock (FleetLock)
        {
            MultiVehicleAligned = MultiVehicleFleet.Count == Conf.MultiVehicleFleetNum &&
                                  MultiVehicleFleet.Values.All(v => v.Aligned);
        }

        // 安全门：开启互识别时，本车或任一其它车检测不到邻车则整队停车（速度置零）。
        // ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
        var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
        bool othersDetectOk;
        lock (FleetLock)
            othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
        var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk);

        // H: 自动模式（非手动）必须有有效车队中心——主车由 SLAM 反推、从车依赖主车广播 fleetPosValid。
        //    自动模式整队姿态始终依赖 Detour（与 MultiVehicleSyncUseDetour 无关）：定位全程丢失时强制停车。
        //    （用 Detour 时若定位丢失，getCartLocation() 已先行阻塞，此处再兜底要求有效车队中心。）
        //    手动模式不受限（允许仅靠互识别/遥控行驶）。
        if (canMove && autoMode && Conf.MultiVehicleAutoRequireFleetCenter)
        {
            var fleetCenterValid = fleetPosValid && (!isMaster || TryInferFleetCenter(out _, out _, out _));
            if (!fleetCenterValid)
            {
                canMove = false;
                Hedingben.ToastText("自动模式无有效车队中心（定位丢失），已停车", $"MultiVehicle{CarNum}-autostop");
            }
        }

        if (!canMove)
        {
            fleetVx = 0;
            fleetFrontTh = 0;
            fleetRearTh = 0;
            fleetOmega = 0;
            Hedingben.ToastText(
                $"车队停车：{(!ownDetectOk ? "本车" : "其它车")}2腿检测丢失（速度已置零）",
                $"MultiVehicle{CarNum}-stop");
        }
        else
        {
            Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
        }

        VisualizeFleet(contour, layoutX, layoutY, layoutTh);

        var fleetReady = false;
        var fleetCount = 0;
        lock (FleetLock)
        {
            fleetCount = MultiVehicleFleet.Count;
            fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
        }

        // 补偿量提到块外，便于诊断日志统一记录三类来源（检测/SLAM）的贡献。
        float xDetectCompensate = 0, yDetectCompensate = 0, thDetectCompensate = 0;
        float xPosCompensate = 0, yPosCompensate = 0, thPosCompensate = 0;
        float posBiasX = 0, posBiasY = 0, posBiasTh = 0;

        if (fleetReady)
        {
            chassis.SetOriginBias(layoutX, layoutY, layoutTh);
            // E: 统一控制点半径——配置 >0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。
            var controlRadius = Conf.MultiVehicleControlRadius > 0
                ? Conf.MultiVehicleControlRadius
                : syncDistance / 2f;
            chassis.ControlPointRadius = controlRadius;
            // #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
            chassis.RotateCompTangentFrac = Conf.MultiVehicleRotateCompTangentFrac;

            if (canMove && Conf.MultiVehicleUseDetect)
            {
                xDetectCompensate = Math.Abs(detectDx) > Conf.SingleCarSyncPrecisionXy
                    ? detectDx * Conf.MultiVehicleDetectBiasXFac : 0;
                yDetectCompensate = Math.Abs(detectDy) > Conf.SingleCarSyncPrecisionXy
                    ? detectDy * Conf.MultiVehicleDetectBiasYFac : 0;
                thDetectCompensate = Math.Abs(detectDth) > Conf.SingleCarSyncPrecisionTh
                    ? detectDth * Conf.MultiVehicleDetectBiasThFac : 0;

                xDetectCompensate = ClampBias(xDetectCompensate, Conf.MultiVehicleDetectBiasXThreshold);
                yDetectCompensate = ClampBias(yDetectCompensate, Conf.MultiVehicleDetectBiasYThreshold);
                thDetectCompensate = ClampBias(thDetectCompensate, Conf.MultiVehicleDetectBiasThThreshold);
            }

            // 车队内姿态纠正(POS 补偿)：仅当 useDetourCorrection 开启且本车读到全局定位时施加。
            // 这是"定位参与车队内姿态纠正"的唯一开关点——关闭它不影响上面的整队姿态计算/安全门。
            if (canMove && useDetourCorrection && slamRead && TryInferFleetCenter(out _, out _, out _))
            {
                var supposedPos = LessMath.Transform2D(
                    Tuple.Create(CenterX, CenterY, CenterTh),
                    Tuple.Create(layoutX, layoutY, layoutTh));
                var posBias = LessMath.SolveTransform2D(Tuple.Create(selfX, selfY, selfTh), supposedPos);
                posBiasX = (float)posBias.Item1;
                posBiasY = (float)posBias.Item2;
                posBiasTh = (float)posBias.Item3;

                xPosCompensate = Math.Abs(posBias.Item1) > Conf.SingleCarSyncPrecisionXy
                    ? ClampBias(posBias.Item1 * Conf.MultiVehiclePosBiasXFac, Conf.MultiVehiclePosBiasXThreshold)
                    : 0;
                yPosCompensate = Math.Abs(posBias.Item2) > Conf.SingleCarSyncPrecisionXy
                    ? ClampBias(posBias.Item2 * Conf.MultiVehiclePosBiasYFac, Conf.MultiVehiclePosBiasYThreshold)
                    : 0;
                thPosCompensate = Math.Abs(posBias.Item3) > Conf.SingleCarSyncPrecisionTh
                    ? ClampBias(posBias.Item3 * Conf.MultiVehiclePosBiasThFac, Conf.MultiVehiclePosBiasThThreshold)
                    : 0;
            }

            sendMotionPainter.Clear();
            if (fleetMode == 2)
            {
                // 原地旋转：底盘原点已偏置到车队中心，绕队中心旋转，并叠加 PI 闭环纠偏维持队形。
                // detect/pos 偏差(dx,dy,dth) 本身就是"本车参考点应在车体系移动到的位置/应转的角"，作为误差喂给 PI。
                // 纯 P 对抗恒定横向滑移有稳态残差，积分项消除之。检测丢失(canMove=false)时不纠偏并清空积分。
                float rotCompVx = 0, rotCompVy = 0, rotCompOmega = 0;
                var now = DateTime.Now;
                // dt 限幅，避免首帧/卡顿导致积分突跳
                var dt = _rotPiLastTime == DateTime.MinValue ? 0f
                    : (float)Math.Min(0.2, Math.Max(0, (now - _rotPiLastTime).TotalSeconds));
                _rotPiLastTime = now;

                // 仅在"被指令旋转"时才纠偏：松开摇杆(fleetOmega≈0)时绝不再下发补偿，
                // 否则积分残留会持续驱动车辆平移/旋转，表现为"松杆后轮子来回打、自转停不下来"。
                var rotating = Math.Abs(fleetOmega) > Conf.MultiVehicleRotateActiveOmega;
                if (canMove && rotating)
                {
                    // #3 抗饱和：上一拍舵轮未对齐(gate=0、车没真正转动)时冻结积分，避免卡死时积分越积越大。
                    var allowInteg = chassis.LastRotateAligned;
                    var errX = detectDx + posBiasX;    // mm，车体系：本车纵向(前+)应移动量
                    var errY = detectDy + posBiasY;    // mm，车体系：本车横向(左+)应移动量
                    var errTh = detectDth + posBiasTh; // deg，本车应转角
                    rotCompVx = RotatePiTerm(errX, ref _rotIntegX, Conf.SingleCarSyncPrecisionXy,
                        Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
                        Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
                    rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
                        Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
                        Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
                    rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
                        Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac,
                        Conf.MultiVehicleRotateCompThMax, dt, allowInteg);

                    // #1 纠偏随旋转指令缩放：comp ×= |fleetOmega| / 本次峰值。
                    // 加速+匀速段峰值≈当前 → 系数≈1（全力纠偏，不削弱）；减速段当前<峰值 → 系数随转速同步下降。
                    // 关键：旋转切向也∝转速，故"纠偏:切向"比例全程恒定=匀速段比例（远<1），既杜绝末段轮子乱打方向，
                    // 又不像按切向钳位那样在匀速段就削弱纠偏。
                    var absOmega = Math.Abs(fleetOmega);
                    _rotOmegaPeak = Math.Max(_rotOmegaPeak, absOmega);
                    if (_rotOmegaPeak > 1e-3f)
                    {
                        var compScale = Math.Min(1f, absOmega / _rotOmegaPeak);
                        rotCompVx *= compScale;
                        rotCompVy *= compScale;
                        rotCompOmega *= compScale;
                    }
                }
                else
                {
                    // 检测丢失或未指令旋转：清零补偿并复位积分/计时/峰值，停止时不再有残留驱动。
                    _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                    _rotPiLastTime = DateTime.MinValue;
                    _rotOmegaPeak = 0;
                }

                _mvRotCompVx = rotCompVx;
                _mvRotCompVy = rotCompVy;
                _mvRotCompOmega = rotCompOmega;

                chassis.SendRotateMotion(fleetOmega,
                    localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);

                // 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
                if (isMaster)
                    LogRotatePoseSample();
            }
            else
            {
                _mvRotCompVx = 0;
                _mvRotCompVy = 0;
                _mvRotCompOmega = 0;
                // 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
                _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                _rotPiLastTime = DateTime.MinValue;
                _rotPoseEpisode = false;
                _rotPosePrevTime = DateTime.MinValue;
                // 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
                chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
                    localCompensateX: xDetectCompensate + xPosCompensate,
                    localCompensateY: yDetectCompensate + yPosCompensate,
                    localCompensateTh: thDetectCompensate + thPosCompensate);
            }
        }

        // === 诊断日志（节流 ~300ms）===
        // 用于定位“剧烈运动”来源：BASE=遥控基础速度；DETECT=2腿检测补偿；POS=SLAM编队补偿；SEND=最终下发。
        // 若 BASE≈0 但 SEND 持续非零，说明是补偿在驱动；再看 DETECT/POS 哪一路的 comp 持续非零即定位到来源。
        // 同时落 DLog(tag MultiVehicleDbg) 与屏幕 ToastText(tag MultiVehicle{CarNum}-dbg)，后者无需开启磁盘转储即可直接观察。
        if ((DateTime.Now - _mvDbgLastLog).TotalMilliseconds >= 300)
        {
            _mvDbgLastLog = DateTime.Now;
            var spacingStr = float.IsNaN(currentSpacing) ? "NaN" : currentSpacing.ToString("F0");
            var cx = xDetectCompensate + xPosCompensate;
            var cy = yDetectCompensate + yPosCompensate;
            var cth = thDetectCompensate + thPosCompensate;
            var crabDbg = isMaster && manualEnabled && fleetMode == 1
                ? $"| CRAB in({crabInputVx:F3},{crabInputVy:F3}) raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
                : "";

            var dbg =
                $"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} slam:{slamRead} corr:{useDetourCorrection} fleetPos:{fleetPosValid} " +
                $"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} useDetect:{Conf.MultiVehicleUseDetect} " +
                $"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " +
                crabDbg +
                $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
                $"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} " +
                $"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
                $"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " +
                $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " +
                $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
                $"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " +
                $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2})";

            DLog.Log(dbg, "MultiVehicleDbg");
            FleetDiag(dbg);

            // 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储）
            Hedingben.ToastText(
                $"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " +
                $"| ready{fleetReady} canMove{canMove}",
                $"MultiVehicle{CarNum}-dbg");
            Hedingben.ToastText(
                $"DET v{detectValid} dx{detectDx:F0} dy{detectDy:F0} dth{detectDth:F1} cmp({xDetectCompensate:F0},{yDetectCompensate:F0},{thDetectCompensate:F1}) " +
                $"| POS bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) cmp({xPosCompensate:F0},{yPosCompensate:F0},{thPosCompensate:F1})",
                $"MultiVehicle{CarNum}-dbg2");
        }

        if (isMaster)
        {
            lock (FleetLock)
            {
                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
                    layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
            }

            if (fleetReady)
            {
                VehicleSyncNotification notification;
                lock (FleetLock)
                {
                    notification = new VehicleSyncNotification
                    {
                        // 广播"整队姿态有效"：主车读到全局定位即为真，从车据此放行自动模式安全门。
                        PosAvailable = slamRead,
                        CenterX = CenterX,
                        CenterY = CenterY,
                        CenterTh = CenterTh,
                        Aligned = MultiVehicleAligned,
                        Fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet),
                        FleetVx = fleetVx,
                        FleetFrontTh = fleetFrontTh,
                        FleetRearTh = fleetRearTh,
                        Mode = fleetMode,
                        FleetOmega = fleetOmega,
                        AutoEnabled = MultiVehicleAutoEnabled,
                        // 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
                        ManualEnabled = manualEnabled,
                        SyncTh = syncTh,
                        SyncDistance = syncDistance,
                        DeltaDetectCenter = deltaDetectCenter,
                        // F: 单调递增序列号（从 1 起），从车据此丢弃乱序旧包。
                        Seq = ++_multiVehicleNotifySeq,
                        // D: 透传理想车队中心（仅自动模式且控制器给出时有效）。
                        HasIdeal = !manualEnabled && MultiVehicleAutoEnabled &&
                                   Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal,
                        IdealX = MultiVehicleAutoIdealX,
                        IdealY = MultiVehicleAutoIdealY,
                        IdealTh = MultiVehicleAutoIdealTh
                    };
                }

                foreach (var kv in notification.Fleet)
                {
                    var ip = kv.Value.Ip;
                    var port = kv.Value.Port > 0 ? kv.Value.Port : WebAPI.port;
                    if (string.IsNullOrWhiteSpace(ip) || kv.Key == CarNum)
                        continue;
                    FireAndForgetNotify(hc, ip, port, notification);
                }
            }
        }
        else
        {
            FireAndForgetRegister(hc, BuildSelfInfo(false, slamRead, selfX, selfY, selfTh,
                layoutX, layoutY, layoutTh, selfAligned, ownDetectOk));
        }
    }

    private bool IsMultiVehicleMaster() => Conf.MultiVehicleMasterEndpoint == "/";

    /// <summary>
    /// C: 剔除超过 TTL 未刷新（register/notify）的 fleet 成员。调用方须已持有 FleetLock。
    /// 从车崩溃/断网后其条目过期被删除，使 fleetReady(数量==总数) 同时蕴含"全部成员在线且新鲜"，
    /// 主车不再基于过期 layout/DetectOk 继续 SendMotion+notify。
    /// </summary>
    private void PruneStaleFleetMembers()
    {
        var ttlMs = Conf.MultiVehicleMemberTtlMs > 0
            ? Conf.MultiVehicleMemberTtlMs
            : Math.Max(500, Conf.MultiVehicleSyncInterval * 6);
        var now = DateTime.Now;
        var stale = MultiVehicleFleet.Keys
            .Where(k => k != CarNum &&
                        (!_multiVehicleFleetSeen.TryGetValue(k, out var seen) ||
                         (now - seen).TotalMilliseconds > ttlMs))
            .ToList();
        foreach (var k in stale)
        {
            MultiVehicleFleet.Remove(k);
            _multiVehicleFleetSeen.Remove(k);
            Hedingben.ToastText($"剔除掉线成员 car{k}（{ttlMs:F0}ms 未刷新）", $"MultiVehicle{CarNum}-prune");
        }
    }

    private bool FleetHasPosAvailable()
    {
        lock (FleetLock)
            return MultiVehicleFleet.Values.Any(v => v.PosAvailable);
    }

    // 基于 SLAM 世界坐标的最近邻车间距（mm）；无可用定位邻车时返回 NaN。
    private float TryGetSlamSpacing(float selfX, float selfY)
    {
        List<VehicleSyncInfo> others;
        lock (FleetLock)
            others = MultiVehicleFleet
                .Where(kv => kv.Key != CarNum && kv.Value.PosAvailable)
                .Select(kv => kv.Value).ToList();

        if (others.Count == 0) return float.NaN;
        return others.Min(o => (float)Math.Sqrt((o.X - selfX) * (o.X - selfX) + (o.Y - selfY) * (o.Y - selfY)));
    }

    private bool TryInferFleetCenter(out float centerX, out float centerY, out float centerTh)
    {
        centerX = centerY = centerTh = 0;
        List<VehicleSyncInfo> positioned;
        lock (FleetLock)
            positioned = MultiVehicleFleet.Values.Where(v => v.PosAvailable).ToList();

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

        var centers = positioned.Select(InferFleetCenterFromCar).ToList();
        centerX = centers.Average(c => c.Item1);
        centerY = centers.Average(c => c.Item2);
        var avgSin = centers.Average(c => Math.Sin(c.Item3 * Math.PI / 180.0));
        var avgCos = centers.Average(c => Math.Cos(c.Item3 * Math.PI / 180.0));
        centerTh = (float)(Math.Atan2(avgSin, avgCos) * 180.0 / Math.PI);
        return true;
    }

    private static (float, float, float) InferFleetCenterFromCar(VehicleSyncInfo car)
    {
        // 由 carWorld = Transform2D(center, layout) 反推 center = carWorld ∘ layout⁻¹。
        // 注意 layout⁻¹ 是真正的 SE(2) 逆，而非逐分量取负 (-layout)：
        // 当 layoutTh≠0（如从车 180°）时，逐分量取负会把平移方向算错，导致车队中心偏移 → 位姿补偿跑飞。
        var layout = Tuple.Create((double)car.LayoutX, (double)car.LayoutY, (double)car.LayoutTh);
        var layoutInv = LessMath.SolveTransform2D(layout, Tuple.Create(0.0, 0.0, 0.0));
        var center = LessMath.Transform2D(
            Tuple.Create((double)car.X, (double)car.Y, (double)car.Th),
            layoutInv);
        return ((float)center.Item1, (float)center.Item2, (float)center.Item3);
    }

    /// <summary>
    /// 由本车（通常为主车）当前 Detour SLAM 位姿反推车队中心位姿（世界系）。
    /// 复用 GetLayoutPose + InferFleetCenterFromCar 的同款 SE(2) 反推（center = carWorld ∘ layout⁻¹），
    /// 保证与自动模式车队中心计算一致。供动作（如 FleetCrabWalk）取路径起点用；无有效定位时返回 false。
    /// </summary>
    public bool TryGetFleetCenterFromSlam(out float centerX, out float centerY, out float centerTh)
    {
        centerX = centerY = centerTh = 0;
        var carPos = DetourInterface.getCartLocation();
        var (layoutX, layoutY, layoutTh) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
        var self = new VehicleSyncInfo
        {
            X = (float)carPos.x,
            Y = (float)carPos.y,
            Th = (float)carPos.th,
            LayoutX = layoutX,
            LayoutY = layoutY,
            LayoutTh = layoutTh
        };
        var (cx, cy, cth) = InferFleetCenterFromCar(self);
        centerX = cx;
        centerY = cy;
        centerTh = cth;
        return true;
    }

    /// <summary>
    /// 预热：用主车 SLAM 位姿，立即(1) 对外发布有效"车队中心快照"，(2) 把主车自身以 posAvailable=true
    /// 写入 fleet 表。供 FleetCrabWalk 等动作在启动控制器 Track() 前调用，解决两个启动期问题：
    ///  - 控制器首帧通过 MultiVehicleGetFleetPos 读到 (0,0,0) → 误判已到终点、立即结束；
    ///  - 自动联动循环的安全门 FleetHasPosAvailable（PilotDefinition.cs ~402）在冷启动(无车上报定位)时
    ///    会把 AutoEnabled 关掉、循环无法发布真实中心。播种主车 posAvailable 条目即可越过该门，使循环正常接管。
    /// 需在主车上调用；getCartLocation() 无定位时会阻塞（与自动模式一致）。
    /// </summary>
    public bool PrimeMasterAutoFromSlam()
    {
        var carPos = DetourInterface.getCartLocation();
        var (lx, ly, lth) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
        var info = BuildSelfInfo(true, true, (float)carPos.x, (float)carPos.y, (float)carPos.th,
            lx, ly, lth, false);
        var (cx, cy, cth) = InferFleetCenterFromCar(info);
        PublishFleetCenter(cx, cy, cth);
        lock (FleetLock)
        {
            MultiVehicleFleet[CarNum] = info;
            _multiVehicleFleetSeen[CarNum] = DateTime.Now;
        }
        return true;
    }

    private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
    {
        // 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
        // 车体相对车队中心沿编队方向 ±syncDistance/2 对称分布，从车额外朝向翻转 180°。
        var rad = syncTh / 180f * Math.PI;
        var sign = CarNum == 1 ? 1f : -1f;
        var xx = (float)(Math.Cos(rad) * syncDistance / 2 * sign);
        var yy = (float)(Math.Sin(rad) * syncDistance / 2 * sign);
        return (xx, yy, syncTh + (CarNum == 1 ? 0 : 180));
    }

    private VehicleSyncInfo BuildSelfInfo(bool master, bool posAvailable, float x, float y, float th,
        float layoutX, float layoutY, float layoutTh, bool aligned, bool detectOk = true)
    {
        return new VehicleSyncInfo
        {
            Master = master,
            Ip = Conf.SimpleIp,
            Port = WebAPI.port,
            PosAvailable = posAvailable,
            X = x,
            Y = y,
            Th = th,
            LayoutX = layoutX,
            LayoutY = layoutY,
            LayoutTh = layoutTh,
            Aligned = aligned,
            DetectOk = detectOk
        };
    }

    private void VisualizeFleet(List<Vector2> contour, float egoLayoutX, float egoLayoutY, float egoLayoutTh)
    {
        // 画在本车车体系：global=false，渲染时由 ClumsyLite 叠加本车真实位姿。
        // 每个成员按“相对本车 layout 的位姿”绘制（memberInEgo = egoLayout⁻¹ ∘ memberLayout），
        // 这样手动模式无 SLAM 定位也能正确显示编队相对关系；避免之前用车队系 layout 坐标叠加本车位姿造成的整体平移。
        var fleetPainter = UI.GetPainter("MultiVehicleFleet-vis", false);
        Dictionary<int, VehicleSyncInfo> fleet;
        lock (FleetLock)
            fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet);

        var egoLayout = Tuple.Create((double)egoLayoutX, (double)egoLayoutY, (double)egoLayoutTh);

        Tuple<double, double, double> MemberInEgo(float mx, float my, float mth)
            => LessMath.SolveTransform2D(egoLayout, Tuple.Create((double)mx, (double)my, (double)mth));

        void DrawContour(Color color, Tuple<double, double, double> rel)
        {
            var transformed = contour
                .Select(pp => LessMath.Transform2D((float)rel.Item1, (float)rel.Item2, (float)rel.Item3, pp))
                .ToList();
            for (var i = 0; i < 4; ++i)
                fleetPainter.DrawLine(color, transformed[i], transformed[(i + 1) % 4]);
            // 对角线便于辨认朝向
            fleetPainter.DrawLine(color, transformed[0], transformed[2]);
            fleetPainter.DrawLine(color, transformed[1], transformed[3]);
        }

        fleetPainter.Clear();
        foreach (var kvp in fleet)
        {
            var color = kvp.Value.Master ? Color.Red : Color.Orange;
            var rel = MemberInEgo(kvp.Value.LayoutX, kvp.Value.LayoutY, kvp.Value.LayoutTh);
            DrawContour(color, rel);
            fleetPainter.DrawText(color, $"{kvp.Key}", new Vector((float)rel.Item1, (float)rel.Item2));
        }
    }

    private void FireAndForgetRegister(HttpClient hc, VehicleSyncInfo info)
    {
        ParseMasterEndpoint(out var masterIp, out var masterPort);
        var infoJson = JsonConvert.SerializeObject(info);
        var url =
            $"http://{masterIp}:{masterPort}/multi-vehicle-register?CarNum={CarNum}&Info={Uri.EscapeDataString(infoJson)}";
        _ = hc.GetStringAsync(url).ContinueWith(t =>
        {
            if (t.IsFaulted)
                DLog.Log($"register failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
        }, TaskScheduler.Default);
    }

    private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
    {
        // F: POST + JSON body，payload 不再受 URL 长度限制；fire-and-forget 但记录失败。
        var notifyJson = JsonConvert.SerializeObject(notification);
        var url = $"http://{ip}:{port}/multi-vehicle-notify";
        var content = new StringContent(notifyJson, System.Text.Encoding.UTF8, "application/json");
        _ = hc.PostAsync(url, content).ContinueWith(t =>
        {
            content.Dispose();
            if (t.IsFaulted)
                DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
        }, TaskScheduler.Default);
    }

    private void ParseMasterEndpoint(out string ip, out int port)
    {
        ip = "127.0.0.1";
        port = 8008;
        var endpoint = Conf.MultiVehicleMasterEndpoint ?? "/";
        if (endpoint == "/") return;
        var parts = endpoint.Split(':');
        if (parts.Length >= 1 && !string.IsNullOrWhiteSpace(parts[0]))
            ip = parts[0];
        if (parts.Length >= 2 && int.TryParse(parts[1], out var p))
            port = p;
    }

    private static float ClampBias(float value, float threshold)
        => Math.Sign(value) * Math.Min(Math.Abs(value), threshold);

    /// <summary>
    /// 原地旋转纠偏单轴 PI 控制器。err 为车体系偏差(mm 或 deg)，输出为修正速度(mm/s 或 deg/s)，
    /// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。
    /// </summary>
    private static float RotatePiTerm(float err, ref float integ, float deadband,
        float pFac, float iFac, float max, float dt, bool allowIntegrate = true)
    {
        // 死区内：不再累加误差，仅输出已积累的积分项(维持对恒定扰动的稳态补偿)。
        if (Math.Abs(err) < deadband)
            return ClampBias(integ * iFac, max);

        // 条件积分(抗 windup)：仅当 (a) 允许积分(舵轮已对齐、车在真正转动) 且
        // (b) 总输出未在同向饱和 时才累加误差。
        // allowIntegrate=false：上一拍舵轮未对齐(gate=0、车没动)，此时积分误差是纯 windup，
        // 会让纠偏越积越大、舵轮更对不齐 → 原地卡死；故冻结积分(保留已有值，仅输出 P+已积分)。
        // 同向饱和停积分：起步大误差让 P 顶满 max 时继续积分会顶满积分器，误差反向后泄放慢、造成过冲回拉。
        var unclamped = err * pFac + integ * iFac;
        var saturatedSameSign = Math.Abs(unclamped) >= max && Math.Sign(unclamped) == Math.Sign(err);
        if (allowIntegrate && !saturatedSameSign)
            integ += err * dt;

        var iTerm = integ * iFac;
        // 抗积分饱和：把积分贡献限制在 ±max，并反算回写积分器，避免 windup。
        if (iFac > 1e-9f)
        {
            var iLimit = max / iFac;
            if (integ > iLimit) integ = iLimit;
            else if (integ < -iLimit) integ = -iLimit;
            iTerm = integ * iFac;
        }

        return ClampBias(err * pFac + iTerm, max);
    }

    /// <summary>
    /// 主车专用：通过 Playground WebAPI 读取本车与邻车的真实 sim 位姿，落 RotatePoseDbg 日志。
    /// 量化"开环横向滑移"来源：
    ///   - w1/w2/dw：两车实际偏航角速率(deg/s)。dw 持续非零 ⇒ 主从转速不一致(相对旋转)。
    ///   - rel_in1(x,y,dyaw)：邻车在本车体系下的真实相对位姿；dyaw=实际相对朝向−180°。
    ///     dyaw≈0 但 y 持续漂 ⇒ 纯横向平移滑移(非角度滞后，印证 B 无用)。
    ///   - midDrift：车队几何中心(世界系)相对起转瞬间的位移 ⇒ 整体平移漂移量。
    /// 节流 ~200ms；WebAPI 异常时静默跳过，不影响控制环。
    /// </summary>
    private void LogRotatePoseSample()
    {
        var now = DateTime.Now;
        if ((now - _rotPoseLastLog).TotalMilliseconds < 200) return;
        _rotPoseLastLog = now;

        var neighbor = Conf.PlaygroundNeighborRobotName;
        if (string.IsNullOrWhiteSpace(neighbor) || neighbor == Conf.PlaygroundRobotName) return;

        try
        {
            var p1 = PlaygroundWebApi.GetPose(Conf.PlaygroundWebApiUrl, Conf.PlaygroundRobotName);
            var p2 = PlaygroundWebApi.GetPose(Conf.PlaygroundWebApiUrl, neighbor);

            // 邻车在本车(car1)体系下的相对位姿：rel = R(-yaw1)·(p2-p1)
            var ddx = p2.X - p1.X;
            var ddy = p2.Y - p1.Y;
            var th1 = p1.YawDeg * (float)Math.PI / 180f;
            var c = (float)Math.Cos(th1);
            var s = (float)Math.Sin(th1);
            var relX = ddx * c + ddy * s;
            var relY = -ddx * s + ddy * c;
            var relYaw = (float)LessMath.ThDiff(p2.YawDeg - p1.YawDeg, 180f); // 实际相对朝向偏离 180° 的量
            var dist = (float)Math.Sqrt(ddx * ddx + ddy * ddy);

            var midX = (p1.X + p2.X) / 2f;
            var midY = (p1.Y + p2.Y) / 2f;
            if (!_rotPoseEpisode)
            {
                _rotPoseEpisode = true;
                _rotPoseMid0X = midX;
                _rotPoseMid0Y = midY;
            }
            var midDx = midX - _rotPoseMid0X;
            var midDy = midY - _rotPoseMid0Y;

            // 实际偏航角速率（数值微分）
            float w1 = 0, w2 = 0, dw = 0;
            if (_rotPosePrevTime != DateTime.MinValue)
            {
                var dt = (float)(now - _rotPosePrevTime).TotalSeconds;
                if (dt > 1e-3)
                {
                    w1 = (float)LessMath.ThDiff(p1.YawDeg, _rotPosePrevYaw1) / dt;
                    w2 = (float)LessMath.ThDiff(p2.YawDeg, _rotPosePrevYaw2) / dt;
                    dw = w1 - w2;
                }
            }
            _rotPosePrevTime = now;
            _rotPosePrevYaw1 = p1.YawDeg;
            _rotPosePrevYaw2 = p2.YawDeg;

            DLog.Log(
                $"car1({p1.X:F0},{p1.Y:F0},{p1.YawDeg:F2}) car2({p2.X:F0},{p2.Y:F0},{p2.YawDeg:F2}) " +
                $"| rel_in1(x:{relX:F0} y:{relY:F0} dyaw:{relYaw:F2}) dist:{dist:F0} " +
                $"| mid({midX:F0},{midY:F0}) midDrift(dx:{midDx:F0} dy:{midDy:F0} |d|:{Math.Sqrt(midDx * midDx + midDy * midDy):F0}) " +
                $"| w1:{w1:F2} w2:{w2:F2} dw:{dw:F2}",
                "RotatePoseDbg");
        }
        catch (Exception e)
        {
            // 诊断用途，WebAPI 不通时静默（仅偶发提示），不打断控制环。
            Hedingben.ToastText($"RotatePose WebAPI err: {e.Message}", $"MultiVehicle{CarNum}-rotpose");
        }
    }

    #endregion
}