using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Commons.Controllers;
using MDCSToolBox.Clumsy.Tracks;
using MyParking.Shared;
using System;
using System.Collections.Generic;
using System.Numerics;
using System.Threading;

namespace MultiWheelC
{
    internal static class MovementTestPreparation
    {
        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
        public static bool AlignWheelsForward(
            ref DriveTask activeTask)
        {
            var preparation = new PrepareWheelsForward();
            var task = new DriveTask(preparation.Get());
            activeTask = task;

            try
            {
                task.Wait();
                return preparation.Completed;
            }
            catch (Exception ex)
            {
                Console.WriteLine(
                    $"测试前舵轮回正失败：{ex.Message}");
                return false;
            }
            finally
            {
                task.Stop();
                if (ReferenceEquals(activeTask, task))
                    activeTask = null;
            }
        }

        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
        public static bool AreWheelsForward(
            float toleranceDegrees = 2f)
        {
            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                Console.WriteLine(
                    "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
                return false;
            }

            try
            {
                var adapter = new MultiWheelChassisAdapter(
                    chassis,
                    PilotDefinition.Self.CarNum);
                var toleranceRadians =
                    AngleMath.DegreesToRadians(toleranceDegrees);

                if (adapter.AreParallelWheelsAligned(
                        0.0,
                        toleranceRadians))
                {
                    return true;
                }

                Console.WriteLine(
                    "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
                return false;
            }
            catch (Exception ex)
            {
                Console.WriteLine(
                    $"检查舵轮方向失败：{ex.Message}");
                return false;
            }
        }
    }

    [MovementTest(name = "准备：四个舵轮与车头方向一致")]
    public class AlignWheelsForwardTest : MovementTest
    {
        private DriveTask _task;

        // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
        public override void Test()
        {
            MovementTestPreparation.AlignWheelsForward(
                ref _task);
        }

        // 停止正在执行的舵轮回正任务并清零底盘运动命令。
        public override void TestStop()
        {
            _task?.Stop();
            _task = null;
        }
    }

    [MovementTest(name = "SendMotion：连续前进4m")]
    public class TestForward4m : MovementTest
    {
        public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
        public float CruiseSpeed = 0.3f;  // 巡航速度上限，单位m/s。
        public int TrialNumber = 1; // 重复实验编号。
        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;
        // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
        public override void Test()
        {
            if (!MovementTestPreparation.AreWheelsForward())
            {
                return;
            }

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消连续前进4m测试。");
                return;
            }
            var source = new Vector2((float)location.x, (float)location.y);
            // Detour航向单位是度，三角函数需要弧度。
            var headingRadians =
                AngleMath.DegreesToRadians(location.th);
            var destination = new Vector2(
                source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
                source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
            _recorder =
                        new TrackingExperimentRecorder(
                            controllerName: "LegacyGeometricController",
                            trajectoryName: "LegacyStraight4m",
                            trialNumber: TrialNumber,
                            referenceStart: source,
                            referenceEnd: destination,
                            referenceSpeed: CruiseSpeed);
            _recorder.Start();
            try
            {
                _task = new DriveTask(
                new DstTracker
                {
                    Src = source,
                    Dst = destination,
                    CarDirectionBias = 0f,
                    MaxSpeed = CruiseSpeed
                }.Get());
                _task.Wait();
                // 保留少量停车后数据，便于观察速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }
    }

    public abstract class InPlaceRotateTestBase : MovementTest
    {
        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;
        private readonly string _trajectoryName;

        protected InPlaceRotateTestBase(
            float relativeAngleDegrees,
            string trajectoryName)
        {
            RelativeAngleDegrees =
                relativeAngleDegrees;
            _trajectoryName =
                trajectoryName;
        }

        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
        public override void Test()
        {
            if (float.IsNaN(RelativeAngleDegrees) ||
                float.IsInfinity(RelativeAngleDegrees) ||
                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
                MaxAngularSpeedDegreesPerSecond <= 0f)
            {
                Console.WriteLine("原地旋转测试参数无效。");
                return;
            }

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消原地旋转测试。");
                return;
            }

            var rotationCenter =
                new Vector2((float)location.x, (float)location.y);
            var targetWorldAngle =
                (float)AngleMath.NormalizeDegrees(
                    location.th + RelativeAngleDegrees);

            _recorder = new TrackingExperimentRecorder(
                controllerName: "InPlaceRotatePID",
                trajectoryName: _trajectoryName,
                trialNumber: TrialNumber,
                referenceStart: rotationCenter,
                referenceEnd: rotationCenter,
                referenceSpeed: 0f,
                referenceAngularSpeed:
                    (float)AngleMath.DegreesToRadians(
                        MaxAngularSpeedDegreesPerSecond));
            _recorder.Start();

            try
            {
                _task = new DriveTask(
                    new MultiWheelRotateInPlace
                    {
                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
                        AngleTarget = targetWorldAngle,
                        PidparamsRead = () => new PIDParams
                        {
                            Kp =
                                PilotDefinition.Conf.InPlaceRotateKp,
                            Ki =
                                PilotDefinition.Conf.InPlaceRotateKi,
                            Kd =
                                PilotDefinition.Conf.InPlaceRotateKd,
                            DeadZone =
                                PilotDefinition.Conf
                                    .InPlaceRotateArriveDeg,
                            SpeedAccPerSec =
                                PilotDefinition.Conf.InPlaceRotateAcc,
                            OutputUpperThreshold =
                                MaxAngularSpeedDegreesPerSecond,
                            MaxI =
                                PilotDefinition.Conf.InPlaceRotateMaxI
                        },
                        CommandAngularSpeedObserver =
                            commandAngularSpeed =>
                                _recorder?.UpdateCommand(
                                    0f,
                                    (float)AngleMath.DegreesToRadians(
                                        commandAngularSpeed))
                    }.Get());

                _task.Wait();

                // 保留少量停止后的样本，用于观察角速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        // 停止原地旋转并保存当前已经采集的实验数据。
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }

    }

    [MovementTest(name = "SendXYThSpeed：原地自转90°")]
    public sealed class TestRotate90 :
        InPlaceRotateTestBase
    {
        public TestRotate90()
            : base(90f, "Rotate90")
        {
        }
    }

    [MovementTest(name = "SendXYThSpeed：原地自转180°")]
    public sealed class TestRotate180 :
        InPlaceRotateTestBase
    {
        public TestRotate180()
            : base(180f, "Rotate180")
        {
        }
    }

    [MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
    public class TestArcMovement : MovementTest
    {
        public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
        public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
        public override void Test()
        {
            if (float.IsNaN(RadiusMillimeters) ||
                float.IsInfinity(RadiusMillimeters) ||
                RadiusMillimeters <= 0f ||
                float.IsNaN(CruiseSpeed) ||
                float.IsInfinity(CruiseSpeed) ||
                CruiseSpeed <= 0f)
            {
                Console.WriteLine("圆弧运动测试参数无效。");
                return;
            }

            if (!MovementTestPreparation.AreWheelsForward())
            {
                return;
            }

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消圆弧运动测试。");
                return;
            }

            var source =
                new Vector2((float)location.x, (float)location.y);
            var headingRadians =
                AngleMath.DegreesToRadians(location.th);

            // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
            var center = new Vector2(
                source.X -
                    RadiusMillimeters *
                    (float)Math.Sin(headingRadians),
                source.Y +
                    RadiusMillimeters *
                    (float)Math.Cos(headingRadians));

            // 从圆心指向车辆起点的极角，比车辆切线航向小90°。
            var startRadialAngleDegrees =
                (float)location.th - 90f;

            var controller = new ChassisController
            {
                BaseSpeed = CruiseSpeed
            }.Get();
            controller.FinishSpeed = 0f;

            var arc = new CircularArcTrack(
                center,
                RadiusMillimeters,
                startRadialAngleDegrees,
                startRadialAngleDegrees + 90f,
                direction: 1)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };

            // 左转90°后，圆心到终点的径向方向等于起始车头方向。
            var destination = center + new Vector2(
                RadiusMillimeters *
                    (float)Math.Cos(headingRadians),
                RadiusMillimeters *
                    (float)Math.Sin(headingRadians));

            if (!controller.AddTrack(arc, "LeftArc90Degrees"))
            {
                Console.WriteLine(
                    "左转90°圆弧轨迹添加失败，取消测试。");
                return;
            }

            _recorder = new TrackingExperimentRecorder(
                controllerName: "LegacyGeometricController",
                trajectoryName:
                    $"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed);
            _recorder.Start();

            try
            {
                _task = new DriveTask(controller.Track());
                _task.Wait();

                // 保留少量停车后的样本，用于观察速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        // 停止圆弧运动并保存当前已经采集的实验数据。
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }
    }

    #region 蟹行运动测试
    [MovementTest(name = "SendMotion：蟹行直线4m")]
    public class TestCrabForward4m : MovementTest
    {
        public float DistanceMillimeters = 4000f;
        public float CruiseSpeed = 0.2f;
        public int TrialNumber = 1;

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
        public override void Test()
        {
            if (!TryReadStartPose(
                    out var source,
                    out var bodyYawRadians))
                return;

            var motionYaw =
                bodyYawRadians + Math.PI / 2.0;
            var destination = new Vector2(
                source.X +
                    DistanceMillimeters *
                    (float)Math.Cos(motionYaw),
                source.Y +
                    DistanceMillimeters *
                    (float)Math.Sin(motionYaw));

            var tracker = new CrabMotionFrameTracker
            {
                CommandBackend =
                    CrabMotionFrameTracker
                        .ChassisCommandBackend
                        .SendMotion,
                PathKind =
                    CrabMotionFrameTracker
                        .ReferencePathKind.Straight,
                StartPosition = source,
                InitialBodyYawRadians =
                    bodyYawRadians,
                LengthMillimeters =
                    DistanceMillimeters,
                CruiseSpeed = CruiseSpeed
            };

            _recorder = new TrackingExperimentRecorder(
                controllerName:
                    "CrabSendMotionTracker",
                trajectoryName:
                    "CrabStraight4m",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed,
                referenceMotionFrameYawDegrees: 90f);
            tracker.CommandObserver =
                (vx, vy, omega) =>
                    _recorder?.UpdateBodyCommand(
                        vx,
                        vy,
                        omega);
            _recorder.Start();

            try
            {
                _task = new DriveTask(tracker.Get());
                _task.Wait();
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateBodyCommand(
                    0f,
                    0f,
                    0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateBodyCommand(
                0f,
                0f,
                0f);
            _recorder?.StopAndSave();
        }

        // 读取并校验测试开始时的Detour世界位姿。
        private static bool TryReadStartPose(
            out Vector2 source,
            out double bodyYawRadians)
        {
            var location =
                DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消蟹行直线测试。");
                source = Vector2.Zero;
                bodyYawRadians = 0.0;
                return false;
            }

            source = new Vector2(
                (float)location.x,
                (float)location.y);
            bodyYawRadians =
                AngleMath.DegreesToRadians(location.th);
            return true;
        }
    }

    [MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
    public class TestCrabLeftArc90 : MovementTest
    {
        public float RadiusMillimeters = 2000f;
        public float CruiseSpeed = 0.2f;
        public int TrialNumber = 1;

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
        public override void Test()
        {
            var location =
                DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消蟹行圆弧测试。");
                return;
            }

            var source = new Vector2(
                (float)location.x,
                (float)location.y);
            var bodyYawRadians =
                AngleMath.DegreesToRadians(location.th);
            var tracker = new CrabMotionFrameTracker
            {
                CommandBackend =
                    CrabMotionFrameTracker
                        .ChassisCommandBackend
                        .SendMotion,
                PathKind =
                    CrabMotionFrameTracker
                        .ReferencePathKind.LeftArc,
                StartPosition = source,
                InitialBodyYawRadians =
                    bodyYawRadians,
                RadiusMillimeters =
                    RadiusMillimeters,
                ArcSweepRadians = Math.PI / 2.0,
                CruiseSpeed = CruiseSpeed
            };
            var destination =
                tracker.GetArcDestination();

            _recorder = new TrackingExperimentRecorder(
                controllerName:
                    "CrabSendMotionTracker",
                trajectoryName:
                    $"CrabLeftArc90_R{RadiusMillimeters:0}mm",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed,
                referenceMotionFrameYawDegrees: 90f);
            tracker.CommandObserver =
                (vx, vy, omega) =>
                    _recorder?.UpdateBodyCommand(
                        vx,
                        vy,
                        omega);
            _recorder.Start();

            try
            {
                _task = new DriveTask(tracker.Get());
                _task.Wait();
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateBodyCommand(
                    0f,
                    0f,
                    0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateBodyCommand(
                0f,
                0f,
                0f);
            _recorder?.StopAndSave();
        }
    }

    [MovementTest(name = "SendMotion：4m S型曲线")]
    public class TestSCurve4m : MovementTest
    {
        public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
        public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
        public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
        public override void Test()
        {
            if (float.IsNaN(LengthMillimeters) ||
                float.IsInfinity(LengthMillimeters) ||
                LengthMillimeters <= 0f ||
                float.IsNaN(LateralOffsetMillimeters) ||
                float.IsInfinity(LateralOffsetMillimeters) ||
                LateralOffsetMillimeters <= 0f ||
                float.IsNaN(CruiseSpeed) ||
                float.IsInfinity(CruiseSpeed) ||
                CruiseSpeed <= 0f)
            {
                Console.WriteLine("S型曲线测试参数无效。");
                return;
            }

            if (!MovementTestPreparation.AreWheelsForward())
                return;

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消4m S型曲线测试。");
                return;
            }

            var source =
                new Vector2((float)location.x, (float)location.y);
            var headingRadians =
                AngleMath.DegreesToRadians(location.th);
            var length = LengthMillimeters;
            var offset = LateralOffsetMillimeters;

            // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
            // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
            var firstControlPoints = new List<Vector2>
            {
                LocalToWorld(source, headingRadians, 0f, 0f),
                LocalToWorld(
                    source, headingRadians,
                    length / 12f, 0f),
                LocalToWorld(
                    source, headingRadians,
                    length / 6f, offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 0.25f, offset)
            };
            var secondControlPoints = new List<Vector2>
            {
                LocalToWorld(
                    source, headingRadians,
                    length * 0.25f, offset),
                LocalToWorld(
                    source, headingRadians,
                    length / 3f, offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 2f / 3f, -offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 0.75f, -offset)
            };
            var thirdControlPoints = new List<Vector2>
            {
                LocalToWorld(
                    source, headingRadians,
                    length * 0.75f, -offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 5f / 6f, -offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 11f / 12f, 0f),
                LocalToWorld(
                    source, headingRadians,
                    length, 0f)
            };

            var firstTrack = new BezierTrack(firstControlPoints)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };
            var secondTrack = new BezierTrack(secondControlPoints)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };
            var thirdTrack = new BezierTrack(thirdControlPoints)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };

            var controller = new ChassisController
            {
                BaseSpeed = CruiseSpeed
            }.Get();
            controller.FinishSpeed = 0f;

            if (!controller.AddTrack(
                    firstTrack,
                    "SCurve4m-Part1") ||
                !controller.AddTrack(
                    secondTrack,
                    "SCurve4m-Part2") ||
                !controller.AddTrack(
                    thirdTrack,
                    "SCurve4m-Part3"))
            {
                Console.WriteLine(
                    "4m S型曲线轨迹添加失败，取消测试。");
                return;
            }

            var destination =
                LocalToWorld(
                    source,
                    headingRadians,
                    length,
                    0f);
            _recorder = new TrackingExperimentRecorder(
                controllerName: "LegacyGeometricController",
                trajectoryName:
                    $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed);
            _recorder.Start();

            try
            {
                _task = new DriveTask(controller.Track());
                _task.Wait();

                // 保留少量停止后的数据，用于观察速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        // 停止S型曲线测试并保存当前已经采集的数据。
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }

        // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
        private static Vector2 LocalToWorld(
            Vector2 origin,
            double headingRadians,
            float localX,
            float localY)
        {
            var cos = (float)Math.Cos(headingRadians);
            var sin = (float)Math.Sin(headingRadians);

            return new Vector2(
                origin.X + localX * cos - localY * sin,
                origin.Y + localX * sin + localY * cos);
        }
    }

    #endregion
    
    #region 夹臂运动测试
    public abstract class ClampMovementTestBase : MovementTest
    {
        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。

        private DriveTask _task;
        protected abstract bool Close { get; }

        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
        public override void Test()
        {
            var leftTarget = Close
                ? PilotDefinition.Self.LeftArmUpperPos
                : PilotDefinition.Self.LeftArmLowerPos;
            var rightTarget = Close
                ? PilotDefinition.Self.RightArmUpperPos
                : PilotDefinition.Self.RightArmLowerPos;

            if (float.IsNaN(leftTarget) ||
                float.IsInfinity(leftTarget) ||
                float.IsNaN(rightTarget) ||
                float.IsInfinity(rightTarget))
            {
                Console.WriteLine(
                    "夹臂目标位置无效，取消夹臂运动测试。");
                return;
            }

            // 防止重复点击时上一项夹臂任务仍在运行。
            TestStop();
            Console.WriteLine(
                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
                $"左目标={leftTarget}，右目标={rightTarget}");

            var task = new DriveTask(
                new ClampToTarget
                {
                    LeftClampTarget = leftTarget,
                    RightClampTarget = rightTarget,
                    TimeoutSeconds = TimeoutSeconds
                }.Get());
            _task = task;

            try
            {
                task.Wait();
            }
            finally
            {
                PilotDefinition.Self.SpeedLeftArm = 0f;
                PilotDefinition.Self.SpeedRightArm = 0f;
                if (ReferenceEquals(_task, task))
                    _task = null;
            }
        }

        // 停止夹臂任务并立即清零左右夹臂下发速度。
        public override void TestStop()
        {
            _task?.Stop();
            _task = null;
            PilotDefinition.Self.SpeedLeftArm = 0f;
            PilotDefinition.Self.SpeedRightArm = 0f;
        }
    }

    [MovementTest(name = "夹臂关闭测试")]
    public sealed class TestClampCloseMovement
        : ClampMovementTestBase
    {
        protected override bool Close => false;
    }

    [MovementTest(name = "夹臂启动测试")]
    public sealed class TestClampOpenMovement
        : ClampMovementTestBase
    {
        protected override bool Close => true;
    }
    #endregion
}