// using System;
// using System.Collections.Generic;
// using System.Numerics;
// using System.Threading;
// using ClumsyCore;
// using ClumsyCore.Interfaces;
// using ClumsyCore.Pilot;
// using FundamentalLib;
// using MDCSToolBox.Clumsy.Movements;
// using MDCSToolBox.Clumsy.Pilot;
// using MDCSToolBox.Clumsy.Tracks;
// using MyParking.Shared;

// namespace MultiWheelC
// {
//     [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();
//         }
//     }

//     [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();
//         }
//     }

//     [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);
//         }
//     }
// }