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.Movements;
using MDCSToolBox.Clumsy.Pilot;

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 lastCenterLog = DateTime.MinValue;
        var cmdMag = 0f;      // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
        var centerTracking = false;
        float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
        float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 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;
        }

        float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
        if (hasPos)
        {
            var startPos = DetourInterface.getCartLocation();
            centerStartCarX = (float)startPos.x;
            centerStartCarY = (float)startPos.y;
            centerStartCarTh = (float)startPos.th;
            prevTh = centerStartCarTh;
            startTh = prevTh;
            if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
                    out centerStartX, out centerStartY, out centerStartTh))
            {
                centerLastX = centerStartX;
                centerLastY = centerStartY;
                centerLastTh = centerStartTh;
                centerMaxDrift = 0;
                centerTracking = true;
            }
        }
        accumulated = 0f;
        start = DateTime.Now;
        lastTime = start;
        lastLog = DateTime.MinValue;
        lastCenterLog = 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");
        if (centerTracking)
        {
            DLog.Log(
                $"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
                $"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
                $"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
                "FleetRotateCenterDbg");
        }

        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)
            {
                var carPos = DetourInterface.getCartLocation();
                curTh = (float)carPos.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;

                if (centerTracking &&
                    self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
                        out centerLastX, out centerLastY, out centerLastTh))
                {
                    var centerDx = centerLastX - centerStartX;
                    var centerDy = centerLastY - centerStartY;
                    var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
                    centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
                    var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
                    if ((now - lastCenterLog).TotalMilliseconds >= 250)
                    {
                        lastCenterLog = now;
                        DLog.Log(
                            $"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
                            $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
                            $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
                            $"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
                            $"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
                            "FleetRotateCenterDbg");
                    }
                }
            }
            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");
        if (centerTracking)
        {
            var centerDx = centerLastX - centerStartX;
            var centerDy = centerLastY - centerStartY;
            var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
            var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
            DLog.Log(
                $"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
                $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
                $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
                $"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
                "FleetRotateCenterDbg");
        }
        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();
    }
}

[MovementTest(name = "车队联动-曲线行走")]
public class FleetCurveWalkTest : MovementTest
{
    private FleetCurveWalk _proc;
    private DriveTask _task;

    public override void Test()
    {
        var self = PilotDefinition.Self;
        if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
            !self.TryGetFleetCenterFromSlam(out x, out y, out th))
        {
            DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
            return;
        }

        if (!FleetCurveWalk.TryParsePointList(PilotDefinition.Conf.FleetCurveControlPoints,
                out var relativePoints, out var error))
        {
            DLog.Log($"FleetCurveWalkTest abort: {error}", "FleetCurveDbg");
            Hedingben.ToastText(error, "FleetCurve");
            return;
        }

        var controlPoints = FleetCurveWalk.BuildRelativeControlPoints(new Vector2(x, y), th, relativePoints);
        _proc = new FleetCurveWalk
        {
            ControlPoints = controlPoints,
            CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
            CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias,
            BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution,
            SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
            FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
            FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
            SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
            StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
        };
        _task = new DriveTask(_proc.Get());
        _task.Wait();
    }

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

[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,
            FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
            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()
    {
    }
}