using System;
using System.Collections.Generic;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using FundamentalLib;
using CommonUsage.Chassis;
using CommonUsage.Mathematics;
using MDCSToolBox.Clumsy.MotionControllers;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MDCSToolBox.Clumsy.Tracks;

namespace MultiWheelC;

public class MultiForwardTest : MovementDefinition
{
    public float Speed = 0.2f;
    public float DurationSeconds = 2f;

    public override IEnumerable<bool> Get()
    {
        var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
        chassis.SetOriginBias(0, 0, 0);
        var end = DateTime.Now.AddSeconds(DurationSeconds);
        while (DateTime.Now < end)
        {
            chassis.SendMotion(Speed, 0, 0);
            yield return true;
        }

        chassis.SendMotion(0, 0, 0);
    }
}

// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
public class MultiRotateToWorldAngle : MovementDefinition
{
    /// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
    public float TargetWorldDeg;

    /// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
    public float RotSpeed = 30f;

    /// <summary>到位角度精度（deg）。</summary>
    public float ArriveDeg = 1f;

    /// <summary>起转前舵轮对齐精度（deg）。</summary>
    public float WheelAlignDeg = 2f;

    public override IEnumerable<bool> Get()
    {
        var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
        chassis.SetOriginBias(0, 0, 0);

        // 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
        while (true)
        {
            chassis.SendRotateMotion(0);
            if (WheelsAligned(chassis, WheelAlignDeg)) break;
            yield return true;
        }

        // 阶段二：旋转到目标世界朝向，到位即停。
        var target = CommonMath.RoundTh(TargetWorldDeg);
        while (true)
        {
            var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
            var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
            if (Math.Abs(diff) <= ArriveDeg) break;
            chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
            yield return true;
        }

        chassis.PredefinedDriveStop();
    }

    private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
    {
#pragma warning disable CS0612, CS0618
        var wheels = chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618
        foreach (var sw in wheels)
            if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
                return false;
        return true;
    }
}

[MovementTest(name = "多舵轮-前进2秒")]
public class MultiForwardMovementTest : MovementTest
{
    private DriveTask _task;

    public override void Test()
    {
        _task = new DriveTask(new MultiForwardTest().Get());
        _task.Wait();
    }

    public override void TestStop() => _task?.Stop();
}

[MovementTest(name = "多舵轮-原地旋转到目标角度")]
public class MultiRotateMovementTest : MovementTest
{
    private DriveTask _task;

    public override void Test()
    {
        _task = new DriveTask(new MultiRotateToWorldAngle
        {
            TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
            RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
            ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
            WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
        }.Get());
        _task.Wait();
    }

    public override void TestStop()
    {
        _task?.Stop();
        ((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
    }
}

// ===== 车队联动-原地旋转动作 =====
// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
//
// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
//          无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
public class FleetRotateInPlace : MovementDefinition
{
    /// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
    public float Omega = 15f;

    /// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
    public float TargetDeltaDeg = 90f;

    /// <summary>到位角度精度(deg)。</summary>
    public float ArriveDeg = 1.5f;

    /// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
    public float SlowDeg = 25f;

    /// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
    public float MinOmega = 3f;

    /// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，&lt;=0 关闭缓启动（阶跃起步）。</summary>
    public float AccelDegPerSec2 = 20f;

    /// <summary>
    /// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
    /// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
    /// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
    /// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
    /// </summary>
    public bool UseDetourHeading = true;

    // 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。

    /// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
    public float SettleSec = 0.5f;

    private void ClearScript()
    {
        var self = PilotDefinition.Self;
        self.MultiVehicleScriptVx = 0;
        self.MultiVehicleScriptVy = 0;
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleScriptEnabled = false;
    }

    public void Stop() => ClearScript();

    public override IEnumerable<bool> Get()
    {
        var self = PilotDefinition.Self;
        var conf = PilotDefinition.Conf;

        if (conf.MultiVehicleMasterEndpoint != "/")
        {
            Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
            yield break;
        }

        var dir = Math.Sign(TargetDeltaDeg);
        if (dir == 0) dir = 1;
        var maxOmega = Math.Abs(Omega);
        var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
        var slowDeg = Math.Max(1e-3f, SlowDeg);                // 减速区宽度
        var accel = AccelDegPerSec2;                           // 缓启动角加速度，仅作用于起步，<=0 关闭
        var targetMag = Math.Abs(TargetDeltaDeg);

        var hasPos = UseDetourHeading;
        var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
        var startTh = prevTh;
        var accumulated = 0f; // 累计带符号转角(deg)
        var start = DateTime.Now;
        var lastTime = start;
        var lastLog = DateTime.MinValue;
        var cmdMag = 0f;      // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
        // 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
        var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
        if (accel > 1e-3) estDuration += maxOmega / (2 * accel);

        DLog.Log(
            $"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
            $"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
            $"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
            "FleetRotateDbg");

        // 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
        // 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
        self.MultiVehicleScriptVx = 0;
        self.MultiVehicleScriptVy = 0;
        self.MultiVehicleScriptMode = 2;
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleScriptEnabled = true;
        self.MultiVehicleRotateWheelsReady = false;
        self.MultiVehicleRotateFleetReady = false;

        DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
        while (!self.MultiVehicleRotateFleetReady)
        {
            self.MultiVehicleScriptVx = 0;
            self.MultiVehicleScriptVy = 0;
            self.MultiVehicleScriptMode = 2;
            self.MultiVehicleScriptVth = 0;
            self.MultiVehicleScriptEnabled = true;
            Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
            yield return true;
        }

        if (hasPos)
        {
            prevTh = (float)DetourInterface.getCartLocation().th;
            startTh = prevTh;
        }
        accumulated = 0f;
        start = DateTime.Now;
        lastTime = start;
        lastLog = DateTime.MinValue;
        cmdMag = 0f;
        DLog.Log(
            $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
            $"fleetAligned={self.MultiVehicleRotateFleetReady}",
            "FleetRotateDbg");

        var stopReason = "stop()";
        while (true)
        {
            var now = DateTime.Now;
            var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
            lastTime = now;
            var elapsed = (now - start).TotalSeconds;

            float desiredMag;
            float curTh = 0f, remaining = 0f, actualRate = 0f;
            if (hasPos)
            {
                curTh = (float)DetourInterface.getCartLocation().th;
                var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
                accumulated += step;
                actualRate = dt > 1e-3 ? step / dt : 0f;            // 实际角速率(deg/s)，用于对比指令
                prevTh = curTh;

                remaining = targetMag - Math.Abs(accumulated);
                if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }

                // 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
                // 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
                desiredMag = remaining < slowDeg
                    ? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
                    : maxOmega;
            }
            else
            {
                // 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
                desiredMag = maxOmega;
                if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
            }

            // 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
            // “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
            if (accel > 1e-3 && desiredMag > cmdMag)
                cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
            else
                cmdMag = desiredMag;

            self.MultiVehicleScriptVth = dir * cmdMag;

            // 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
            if ((now - lastLog).TotalMilliseconds >= 150)
            {
                lastLog = now;
                DLog.Log(
                    hasPos
                        ? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
                          $"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
                        : $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
                    "FleetRotateDbg");
            }

            Hedingben.ToastText(
                hasPos
                    ? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
                    : $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
                "FleetRotate");

            yield return true;
        }

        // 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
        self.MultiVehicleScriptVth = 0;
        var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
        while (DateTime.Now < settleEnd)
            yield return true;

        ClearScript();
        DLog.Log(
            $"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
            $"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
            "FleetRotateDbg");
        Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
    }
}

[MovementTest(name = "车队联动-原地旋转")]
public class FleetRotateInPlaceTest : MovementTest
{
    private FleetRotateInPlace _proc;
    private DriveTask _task;

    public override void Test()
    {
        _proc = new FleetRotateInPlace
        {
            Omega = PilotDefinition.Conf.FleetRotateOmega,
            TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
            ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
            SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
            MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
            AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
            SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
            UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
        };
        _task = new DriveTask(_proc.Get());
        _task.Wait();
    }

    public override void TestStop()
    {
        _proc?.Stop();
        _task?.Stop();
    }
}

// ===== 车队联动-自动蟹行动作 =====
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
//
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
//
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
public class FleetCrabWalk : MovementDefinition
{
    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
    public float CrabAngleDeg = 45f;

    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
    public float BodyToPathAngleDeg = 45f;

    /// <summary>路径长度(mm)。</summary>
    public float CrabLengthMm = 2000f;

    /// <summary>行驶速度(m/s)。</summary>
    public float CrabSpeed = 0.2f;

    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
    public float FleetCrabAccel = 0.2f;

    /// <summary>末端开始减速距离(mm)。</summary>
    public float FleetCrabSlowDistance = 2000f;

    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
    public float FleetCrabFinishDistance = 20f;

    /// <summary>末端最低速度(m/s)。</summary>
    public float FleetCrabFinishSpeed = 0.02f;

    /// <summary>末端减速曲线指数。</summary>
    public float FleetCrabSlowingPow = 0.8f;

    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
    public float GcpThetaThreshold = 95f;

    private bool _stopping;

    private void Cleanup()
    {
        var self = PilotDefinition.Self;
        self.MultiVehicleScriptVx = 0;
        self.MultiVehicleScriptVy = 0;
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleScriptMode = 0;
        self.MultiVehicleScriptEnabled = false;
        self.MultiVehicleAutoVx = 0;
        self.MultiVehicleAutoFrontTh = 0;
        self.MultiVehicleAutoRearTh = 0;
        self.MultiVehicleAutoHasIdeal = false;
        self.MultiVehicleAutoEnabled = false;
    }

    public void Stop()
    {
        _stopping = true;
        Cleanup();
    }

    private static float Clamp(float value, float min, float max)
    {
        if (value < min) return min;
        if (value > max) return max;
        return value;
    }

    private static float ClampAbs(float value, float limit)
    {
        var absLimit = Math.Abs(limit);
        if (absLimit <= 0) return value;
        if (value > absLimit) return absLimit;
        if (value < -absLimit) return -absLimit;
        return value;
    }

    private static float Slew(float current, float target, float maxDelta)
    {
        if (maxDelta <= 0) return target;
        if (target > current + maxDelta) return current + maxDelta;
        if (target < current - maxDelta) return current - maxDelta;
        return target;
    }

    public override IEnumerable<bool> Get()
    {
        var self = PilotDefinition.Self;
        var conf = PilotDefinition.Conf;
        _stopping = false;

        DLog.Log(
            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
            $"autoFields=true pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
            $"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
            "FleetCrabDbg");

        if (conf.MultiVehicleMasterEndpoint != "/")
        {
            DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
            Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
            yield break;
        }

        // 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
        DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
        if (!self.TryGetFleetCenterFromSlam(out var x0, out var y0, out var theta))
        {
            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
            yield break;
        }
        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");

        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
        var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
        var dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0));
        var phiRad = phi / 180.0 * Math.PI;
        var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
        var pathLeft = new Vector2(-pathDir.Y, pathDir.X);

        DLog.Log(
            $"START center=({x0:0},{y0:0},{theta:0.0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
            $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
            $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " +
            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
            "FleetCrabDbg");

        self.MultiVehicleScriptEnabled = false;
        self.MultiVehicleScriptMode = 0;
        self.MultiVehicleScriptVx = 0;
        self.MultiVehicleScriptVy = 0;
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleAutoEnabled = true;
        self.MultiVehicleAutoVx = 0;
        self.MultiVehicleAutoFrontTh = 0;
        self.MultiVehicleAutoRearTh = 0;
        self.MultiVehicleAutoIdealX = x0;
        self.MultiVehicleAutoIdealY = y0;
        self.MultiVehicleAutoIdealTh = targetBodyTh;
        self.MultiVehicleAutoHasIdeal = true;
        self.MultiVehicleAutoCmdTime = DateTime.Now;
        self.PrimeMasterAutoFromSlam();
        DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg");

        var warmEnd = DateTime.Now.AddSeconds(2.0);
        var warmIter = 0;
        var warmReady = false;
        while (!_stopping && DateTime.Now < warmEnd)
        {
            warmIter++;
            self.MultiVehicleScriptEnabled = false;
            self.MultiVehicleScriptMode = 0;
            self.MultiVehicleAutoEnabled = true;
            self.MultiVehicleAutoVx = 0;
            self.MultiVehicleAutoFrontTh = 0;
            self.MultiVehicleAutoRearTh = 0;
            self.MultiVehicleAutoIdealX = x0;
            self.MultiVehicleAutoIdealY = y0;
            self.MultiVehicleAutoIdealTh = targetBodyTh;
            self.MultiVehicleAutoHasIdeal = true;
            self.MultiVehicleAutoCmdTime = DateTime.Now;
            self.PrimeMasterAutoFromSlam();
            var snap = self.GetFleetCenterSnapshot();
            int cnt;
            lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
            if (warmIter % 5 == 0)
                DLog.Log(
                    $"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
                    "FleetCrabDbg");
            if (cnt >= conf.MultiVehicleFleetNum)
            {
                warmReady = true;
                DLog.Log(
                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}",
                    "FleetCrabDbg");
                break;
            }
            yield return true;
        }
        if (!warmReady)
            DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
                "FleetCrabDbg");

        Hedingben.ToastText($"车队蟹行 路径{CrabAngleDeg:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");

        var iter = 0;
        var lastLog = DateTime.MinValue;
        var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
        var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
        var baseSpeed = Math.Abs(CrabSpeed);
        var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
        var accel = Math.Abs(FleetCrabAccel);
        var cmdSpeed = 0f;
        var lastTick = DateTime.Now;
        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
        var holdFrontTh = ClampAbs((float)CommonMath.ThDiff(phi, theta), gcpLimit);
        var holdRearTh = holdFrontTh;
        var stopReason = "done";

        while (!_stopping)
        {
            iter++;

            if (!self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth))
            {
                stopReason = "fleet center invalid";
                DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg");
                break;
            }
            var delta = new Vector2(cx - x0, cy - y0);
            var along = Vector2.Dot(delta, pathDir);
            var lateral = Vector2.Dot(delta, pathLeft);
            var remain = CrabLengthMm - along;
            if (remain <= finishDistance)
                break;

            var targetSpeed = baseSpeed;
            var slowRatio = 1f;
            if (remain < slowDistance)
            {
                slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
                targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
            }
            var now = DateTime.Now;
            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
            lastTick = now;
            var speed = accel > 0 ? Slew(cmdSpeed, targetSpeed, accel * dt) : targetSpeed;
            cmdSpeed = speed;

            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
            var dthItem = ClampAbs(conf.DthLinearFac * headingErr, conf.DthLinearThreshold);
            var biasItem = (float)(-Math.Atan(conf.BiasFac * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
            biasItem = ClampAbs(biasItem, conf.BiasThreshold);
            var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
            var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
            holdFrontTh = frontTh;
            holdRearTh = rearTh;
            var idealAlong = Clamp(along, 0f, CrabLengthMm);
            var ideal = new Vector2(x0, y0) + pathDir * idealAlong;

            self.MultiVehicleScriptEnabled = false;
            self.MultiVehicleScriptMode = 0;
            self.MultiVehicleScriptVx = 0;
            self.MultiVehicleScriptVy = 0;
            self.MultiVehicleScriptVth = 0;
            self.MultiVehicleAutoEnabled = true;
            self.MultiVehicleAutoVx = speed;
            self.MultiVehicleAutoFrontTh = frontTh;
            self.MultiVehicleAutoRearTh = rearTh;
            self.MultiVehicleAutoIdealX = ideal.X;
            self.MultiVehicleAutoIdealY = ideal.Y;
            self.MultiVehicleAutoIdealTh = targetBodyTh;
            self.MultiVehicleAutoHasIdeal = true;
            self.MultiVehicleAutoCmdTime = DateTime.Now;

            if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
            {
                lastLog = DateTime.Now;
                var snap = self.GetFleetCenterSnapshot();
                int fleetCnt;
                lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
                DLog.Log(
                    $"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
                    $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
                    $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
                    $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
                    $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
                    "FleetCrabDbg");
            }
            yield return true;
        }

        if (_stopping)
            stopReason = "stop";

        self.MultiVehicleAutoVx = 0;
        self.MultiVehicleAutoFrontTh = holdFrontTh;
        self.MultiVehicleAutoRearTh = holdRearTh;
        self.MultiVehicleAutoCmdTime = DateTime.Now;
        DLog.Log(
            $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
            "FleetCrabDbg");
        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
        while (!_stopping && DateTime.Now < settleEnd)
        {
            self.MultiVehicleScriptEnabled = false;
            self.MultiVehicleScriptMode = 0;
            self.MultiVehicleAutoEnabled = true;
            self.MultiVehicleAutoVx = 0;
            self.MultiVehicleAutoFrontTh = holdFrontTh;
            self.MultiVehicleAutoRearTh = holdRearTh;
            self.MultiVehicleAutoCmdTime = DateTime.Now;
            yield return true;
        }

        Cleanup();
        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
    }
}

[MovementTest(name = "车队联动-自动蟹行")]
public class FleetCrabWalkTest : MovementTest
{
    private FleetCrabWalk _proc;
    private DriveTask _task;

    public override void Test()
    {
        _proc = new FleetCrabWalk
        {
            CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
            BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
            CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
            CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
            FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
            FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
            FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
            FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
            FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
        };
        _task = new DriveTask(_proc.Get());
        _task.Wait();
    }

    public override void TestStop()
    {
        _proc?.Stop();
        _task?.Stop();
    }
}

// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。

[MovementTest(name = "多舵轮-WebAPI前移")]
public class WebApiForwardMoveTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        var name = PilotDefinition.Conf.PlaygroundRobotName;
        var d = PilotDefinition.Conf.WebApiTranslateMm;

        var pose = PlaygroundWebApi.GetPose(url, name);
        // 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
        var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
        PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
        Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-WebAPI左移")]
public class WebApiLeftMoveTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        var name = PilotDefinition.Conf.PlaygroundRobotName;
        var d = PilotDefinition.Conf.WebApiTranslateMm;

        var pose = PlaygroundWebApi.GetPose(url, name);
        // 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
        var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
        PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
        Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-WebAPI旋转")]
public class WebApiRotateTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        var name = PilotDefinition.Conf.PlaygroundRobotName;
        var deg = PilotDefinition.Conf.WebApiRotateDeg;

        var pose = PlaygroundWebApi.GetPose(url, name);
        var ny = pose.YawDeg + deg; // 逆时针为正
        PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
        Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-WebAPI恢复运动")]
public class WebApiMotionResumeTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        PlaygroundWebApi.ResumeMotion(url);
        Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-WebAPI暂停运动")]
public class WebApiMotionPauseTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
        Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
    }

    public override void TestStop()
    {
    }
}