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 =
                    toleranceDegrees * Math.PI / 180.0;

                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 = location.th * Math.PI / 180.0;
            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();
        }
    }

    [MovementTest(name = "测试原地自转90°")]
    public class TestRotate90 : MovementTest
    {
        public float RelativeAngleDegrees = 90f; // 相对当前航向的旋转角度，逆时针为正。
        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 从当前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 =
                NormalizeDegrees(
                    (float)location.th + RelativeAngleDegrees);

            _recorder = new TrackingExperimentRecorder(
                controllerName: "InPlaceRotatePID",
                trajectoryName: "Rotate90",
                trialNumber: TrialNumber,
                referenceStart: rotationCenter,
                referenceEnd: rotationCenter,
                referenceSpeed: 0f,
                referenceAngularSpeed:
                    MaxAngularSpeedDegreesPerSecond *
                    (float)Math.PI / 180f);
            _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,
                                    commandAngularSpeed *
                                    (float)Math.PI / 180f)
                    }.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();
        }

        // 将世界航向归一化到大约[-180°,180°]。
        private static float NormalizeDegrees(float angleDegrees)
        {
            return (float)(
                angleDegrees -
                Math.Round(angleDegrees / 360.0) * 360.0);
        }
    }

    [MovementTest(name = "旧版SendMotion：左转90°圆弧")]
    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 =
                location.th * Math.PI / 180.0;

            // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
            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();
        }
    }

    [MovementTest(name = "测试蟹行前进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
            {
                PathKind =
                    CrabMotionFrameTracker
                        .ReferencePathKind.Straight,
                StartPosition = source,
                InitialBodyYawRadians =
                    bodyYawRadians,
                LengthMillimeters =
                    DistanceMillimeters,
                CruiseSpeed = CruiseSpeed
            };

            _recorder = new TrackingExperimentRecorder(
                controllerName:
                    "CrabMotionFrameTracker",
                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 =
                location.th * Math.PI / 180.0;
            return true;
        }
    }

    [MovementTest(name = "测试蟹行左转90°圆弧")]
    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 =
                location.th * Math.PI / 180.0;
            var tracker = new CrabMotionFrameTracker
            {
                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:
                    "CrabMotionFrameTracker",
                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 = "测试蟹行4m S型曲线")]
    public class TestCrabSCurve4m : MovementTest
    {
        public float LengthMillimeters = 4000f;
        public float LateralOffsetMillimeters = 400f;
        public float CruiseSpeed = 0.2f;
        public int TrialNumber = 1;

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 将车体左侧作为运动前向，跟踪与普通测试参数一致的4m 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;
            }

            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当前位姿无效，取消蟹行S型曲线测试。");
                return;
            }

            var source = new Vector2(
                (float)location.x,
                (float)location.y);
            var bodyYawRadians =
                location.th * Math.PI / 180.0;
            var initialMotionYaw =
                bodyYawRadians + Math.PI / 2.0;
            var destination = new Vector2(
                source.X +
                    LengthMillimeters *
                    (float)Math.Cos(
                        initialMotionYaw),
                source.Y +
                    LengthMillimeters *
                    (float)Math.Sin(
                        initialMotionYaw));
            var tracker = new CrabMotionFrameTracker
            {
                PathKind =
                    CrabMotionFrameTracker
                        .ReferencePathKind.SCurve,
                StartPosition = source,
                InitialBodyYawRadians =
                    bodyYawRadians,
                LengthMillimeters =
                    LengthMillimeters,
                SCurveLateralOffsetMillimeters =
                    LateralOffsetMillimeters,
                CruiseSpeed = CruiseSpeed
            };

            _recorder = new TrackingExperimentRecorder(
                controllerName:
                    "CrabMotionFrameTracker",
                trajectoryName:
                    $"CrabSCurve4m_A{LateralOffsetMillimeters: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 =
                location.th * Math.PI / 180.0;
            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);
        }
    }

    // C层单车测试：统一使用车体速度命令和SendXYThSpeed跟踪普通模式轨迹。
    public abstract class XYThNormalTrajectoryTestBase : MovementTest
    {
        public float LengthMillimeters = 4000f;
        public float RadiusMillimeters = 2000f;
        public float LateralOffsetMillimeters = 400f;
        public float CruiseSpeed = 0.3f;
        public int TrialNumber = 1;

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        protected abstract CrabMotionFrameTracker.ReferencePathKind ReferencePath { get; }

        protected abstract string TrajectoryName { get; }

        // C层单车测试：读取Detour起点并执行普通模式SendXYThSpeed轨迹。
        public override void Test()
        {
            ValidateParameters();

            if (!TryReadDetourPose(
                    out var source,
                    out var initialBodyYawRadians))
            {
                throw new InvalidOperationException(
                    "Detour当前位置或航向无效，无法开始新版SendXYThSpeed测试。");
            }

            var tracker = new CrabMotionFrameTracker
            {
                PathKind = ReferencePath,
                // 普通模式的运动坐标系与车体坐标系重合。
                MotionFrameYawInBodyRadians = 0.0,
                StartPosition = source,
                InitialBodyYawRadians = initialBodyYawRadians,
                LengthMillimeters = LengthMillimeters,
                RadiusMillimeters = RadiusMillimeters,
                ArcSweepRadians = Math.PI / 2.0,
                SCurveLateralOffsetMillimeters =
                    LateralOffsetMillimeters,
                CruiseSpeed = CruiseSpeed,
            };

            var destination = ReferencePath ==
                              CrabMotionFrameTracker.ReferencePathKind
                                  .LeftArc
                ? tracker.GetArcDestination()
                : new Vector2(
                    source.X +
                    LengthMillimeters *
                    (float)Math.Cos(initialBodyYawRadians),
                    source.Y +
                    LengthMillimeters *
                    (float)Math.Sin(initialBodyYawRadians));

            _recorder = new TrackingExperimentRecorder(
                controllerName: "UnifiedXYThTracker",
                trajectoryName: TrajectoryName,
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed);

            tracker.CommandObserver =
                (vx, vy, omegaRadiansPerSecond) =>
                    _recorder?.UpdateBodyCommand(
                        vx,
                        vy,
                        omegaRadiansPerSecond);

            _recorder.Start();
            _task = new DriveTask(tracker.Get());

            try
            {
                _task.Wait();
            }
            finally
            {
                _recorder?.UpdateBodyCommand(0f, 0f, 0f);
                _recorder?.StopAndSave();
                _recorder = null;
                _task = null;
            }
        }

        // C层单车测试：停止新版SendXYThSpeed轨迹并保存已有记录。
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateBodyCommand(0f, 0f, 0f);
            _recorder?.StopAndSave();
        }

        // C层单车测试：检查新版轨迹的长度、半径、偏移和速度参数。
        private void ValidateParameters()
        {
            if (!IsPositiveFinite(LengthMillimeters))
                throw new ArgumentOutOfRangeException(
                    nameof(LengthMillimeters),
                    "轨迹长度必须是正有限值。");

            if (!IsPositiveFinite(RadiusMillimeters))
                throw new ArgumentOutOfRangeException(
                    nameof(RadiusMillimeters),
                    "圆弧半径必须是正有限值。");

            if (!IsPositiveFinite(LateralOffsetMillimeters))
                throw new ArgumentOutOfRangeException(
                    nameof(LateralOffsetMillimeters),
                    "S型曲线横向偏移必须是正有限值。");

            if (!IsPositiveFinite(CruiseSpeed))
                throw new ArgumentOutOfRangeException(
                    nameof(CruiseSpeed),
                    "巡航速度必须是正有限值。");
        }

        // C层单车测试：读取并验证Detour毫米坐标和角度制航向。
        private static bool TryReadDetourPose(
            out Vector2 position,
            out double yawRadians)
        {
            var location = DetourInterface.getCartLocation();
            var x = location.x;
            var y = location.y;
            var thetaDegrees = location.th;

            position = new Vector2((float)x, (float)y);
            yawRadians = thetaDegrees * Math.PI / 180.0;

            return IsFinite(x) &&
                   IsFinite(y) &&
                   IsFinite(thetaDegrees);
        }

        // C层单车测试：判断浮点参数是否为有限值。
        private static bool IsFinite(double value)
        {
            return !double.IsNaN(value) &&
                   !double.IsInfinity(value);
        }

        // C层单车测试：判断浮点参数是否为正有限值。
        private static bool IsPositiveFinite(double value)
        {
            return IsFinite(value) && value > 0.0;
        }
    }

    [MovementTest(name = "新版SendXYThSpeed：连续前进4m")]
    public sealed class TestXYThForward4m :
        XYThNormalTrajectoryTestBase
    {
        protected override CrabMotionFrameTracker.ReferencePathKind
            ReferencePath =>
            CrabMotionFrameTracker.ReferencePathKind.Straight;

        protected override string TrajectoryName =>
            "XYThStraight4m";
    }

    [MovementTest(name = "新版SendXYThSpeed：左转90°圆弧")]
    public sealed class TestXYThLeftArc90 :
        XYThNormalTrajectoryTestBase
    {
        protected override CrabMotionFrameTracker.ReferencePathKind
            ReferencePath =>
            CrabMotionFrameTracker.ReferencePathKind.LeftArc;

        protected override string TrajectoryName =>
            $"XYThLeftArc90_R{RadiusMillimeters:0}mm";
    }

    [MovementTest(name = "新版SendXYThSpeed：4m S型曲线")]
    public sealed class TestXYThSCurve4m :
        XYThNormalTrajectoryTestBase
    {
        protected override CrabMotionFrameTracker.ReferencePathKind
            ReferencePath =>
            CrabMotionFrameTracker.ReferencePathKind.SCurve;

        protected override string TrajectoryName =>
            $"XYThSCurve4m_A{LateralOffsetMillimeters:0}mm";
    }

    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 TestClampOpenMovement
        : ClampMovementTestBase
    {
        protected override bool Close => false;
    }

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

}






#region 旧版测试
// public abstract class DstTrackerTestBase : MovementTest
// {
//     public bool UseInteractivePick = true;
//     public float srcX;
//     public float srcY;
//     public float dstX;
//     public float dstY;
//     public float carDirectionBias;

//     private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
//     private DriveTask _dt;

//     protected DstTrackerTestBase(float defaultCarDirectionBias)
//     {
//         carDirectionBias = defaultCarDirectionBias;
//     }

//     public override void TestStop()
//     {
//         _dt?.Stop();
//         _painter?.Clear();
//     }

//     public override void Test()
//     {
//         Vector2 p1;
//         Vector2 p2;
//         if (UseInteractivePick)
//         {
//             p1 = UI.GetPoint("point1");
//             p2 = UI.GetPoint("point2");
//         }
//         else
//         {
//             p1 = new Vector2(srcX, srcY);
//             p2 = new Vector2(dstX, dstY);
//         }

//         _painter.Clear();
//         _dt = new DriveTask(new DstTracker
//         {
//             Src = p1,
//             Dst = p2,
//             CarDirectionBias = carDirectionBias,
//         }.Get());
//         _dt.Wait();
//     }
// }

// [MovementTest(name = "测试终点跟踪动作-前进")]
// public sealed class DstTrackerForward : DstTrackerTestBase
// {
//     public DstTrackerForward() : base(0f) { }
// }

// [MovementTest(name = "测试终点跟踪动作-后退")]
// public sealed class DstTrackerBackward : DstTrackerTestBase
// {
//     public DstTrackerBackward() : base(180f) { }
// }

// [MovementTest(name = "底盘旋转测试")]
// public class RotateToAngleTest : MovementTest
// {
//     private DriveTask _dt;

//     // 停止当前正在执行的底盘原地旋转任务。
//     public override void TestStop()
//     {
//         _dt?.Stop();
//     }

//     // 交互输入目标角度后执行底盘原地旋转测试。
//     public override void Test()
//     {
//         var input = UI.GetInput("输入旋转角度：");
//         if (!float.TryParse(input, out var angleTarget))
//         {
//             Console.WriteLine(
//                 $"旋转测试输入无效：{input}");
//             return;
//         }
//         // 防止重复启动测试时，上一项旋转任务仍在运行。
//         _dt?.Stop();
//         var task = new DriveTask(
//             new MultiWheelRotateInPlace
//             {
//                 AngleTarget = angleTarget,

//                 PidparamsRead = () => new PIDParams
//                 {
//                     Kp = PilotDefinition.Conf.InPlaceRotateKp,
//                     Ki = PilotDefinition.Conf.InPlaceRotateKi,
//                     Kd = PilotDefinition.Conf.InPlaceRotateKd,
//                     DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
//                     SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
//                     OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
//                     MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
//                 }
//             }.Get());
//         _dt = task;
//         try
//         {
//             task.Wait();
//         }
//         finally
//         {
//             // 防止旧任务结束时，错误清除后来启动的新任务。
//             if (ReferenceEquals(_dt, task))
//             {
//                 _dt = null;
//             }
//         }
//     }
// }

#endregion