// using ClumsyCore;
// using ClumsyCore.DTools;
// using ClumsyCore.Interfaces;
// using ClumsyCore.Pilot;
// using CommonUsage.Chassis;
// using MyParking.Shared;
// using System;
// using System.Collections.Generic;
// using System.Numerics;

// namespace MultiWheelC
// {
//     // C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
//     public sealed class CrabMotionFrameTracker : MovementDefinition
//     {
//         public enum ReferencePathKind
//         {
//             Straight = 0,
//             LeftArc = 1,
//             SCurve = 2
//         }

//         public enum ChassisCommandBackend
//         {
//             SendXYThSpeed = 0,
//             SendMotion = 1
//         }

//         public ReferencePathKind PathKind;
//         public ChassisCommandBackend CommandBackend =
//             ChassisCommandBackend.SendMotion;
//         public Vector2 StartPosition;
//         public double InitialBodyYawRadians;
//         public float LengthMillimeters = 4000f;
//         public float RadiusMillimeters = 2000f;
//         public float SCurveLateralOffsetMillimeters = 400f;
//         public double ArcSweepRadians = Math.PI / 2.0;
//         public float CruiseSpeed = 0.2f;
//         public float SlowDistanceMillimeters = 600f;
//         public float FinishDistanceMillimeters = 30f;
//         public float MinimumSpeed = 0.04f;
//         public double LateralGainPerSecond = 0.8;
//         public double MaximumLateralCorrection = 0.12;
//         public double HeadingGainPerSecond = 1.5;
//         public double MaximumAngularSpeedRadiansPerSecond =
//             AngleMath.DegreesToRadians(30.0);
//         public double MaximumVirtualSteeringRadians =
//             AngleMath.DegreesToRadians(30.0);
//         public float WheelAlignmentToleranceDegrees = 2f;
//         public float WheelAlignmentStableSeconds = 0.3f;
//         public float WheelAlignmentTimeoutSeconds = 10f;
//         public float TrackingTimeoutSeconds = 60f;
//         public Action<float, float, float> CommandObserver;

//         // 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
//         public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
//         private double _lastSCurveProgress;

//         public override IEnumerable<bool> Get()
//         {
//             ValidateParameters();

//             var chassis =
//                 PilotDefinition.Chassis as MultiWheelChassis;
//             if (chassis == null)
//                 throw new InvalidOperationException(
//                     "当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");

//             var adapter = new MultiWheelChassisAdapter(
//                 chassis,
//                 PilotDefinition.Self.CarNum);
//             adapter.ResetToBodyFrame();

//             var lastCommandTime = DateTime.Now;

//             try
//             {
//                 // 模式切换阶段只转舵轮，驱动速度始终保持为零。
//                 var alignmentStarted = DateTime.Now;
//                 DateTime? stableSince = null;
//                 while (true)
//                 {
//                     if (!adapter.PrepareParallelDirection(
//                             MotionFrameYawInBodyRadians))
//                         throw new InvalidOperationException(
//                             "无法生成运动坐标系对应的舵轮准备姿态。");

//                     var aligned =
//                         adapter.AreParallelWheelsAligned(
//                             MotionFrameYawInBodyRadians,
//                             AngleMath.DegreesToRadians(
//                                 WheelAlignmentToleranceDegrees));

//                     if (aligned)
//                     {
//                         if (stableSince == null)
//                             stableSince = DateTime.Now;

//                         if ((DateTime.Now - stableSince.Value)
//                                 .TotalSeconds >=
//                             WheelAlignmentStableSeconds)
//                             break;
//                     }
//                     else
//                     {
//                         stableSince = null;
//                     }

//                     if ((DateTime.Now - alignmentStarted)
//                             .TotalSeconds >
//                         WheelAlignmentTimeoutSeconds)
//                         throw new TimeoutException(
//                             "舵轮在限定时间内未稳定到达运动坐标系初始方向。");

//                     yield return true;
//                 }

//                 if (CommandBackend ==
//                     ChassisCommandBackend.SendMotion)
//                 {
//                     // 舵轮已按真实机械角度完成预对齐；
//                     // 现在由Shared适配层激活SendMotion虚拟运动坐标系。
//                     adapter.ActivateMotionFrame(
//                         MotionFrameYawInBodyRadians);
//                 }

//                 var trackingStarted = DateTime.Now;
//                 while (true)
//                 {
//                     if ((DateTime.Now - trackingStarted)
//                             .TotalSeconds >
//                         TrackingTimeoutSeconds)
//                         throw new TimeoutException(
//                             "蟹行轨迹在限定时间内未完成。");

//                     var location =
//                         DetourInterface.getCartLocation();
//                     if (!IsFinite(location.x) ||
//                         !IsFinite(location.y) ||
//                         !IsFinite(location.th))
//                         throw new InvalidOperationException(
//                             "蟹行轨迹测试期间Detour位姿无效。");

//                     var currentPosition = new Vector2(
//                         (float)location.x,
//                         (float)location.y);
//                     var currentBodyYaw =
//                         AngleMath.DegreesToRadians(location.th);

//                     CalculateReference(
//                         currentPosition,
//                         out var tangentYaw,
//                         out var referencePoint,
//                         out var remainingMillimeters,
//                         out var referenceCurvature);

//                     if (remainingMillimeters <=
//                         FinishDistanceMillimeters)
//                         break;

//                     var speed =
//                         CalculateSpeed(remainingMillimeters);
//                     var tangent = new Vector2(
//                         (float)Math.Cos(tangentYaw),
//                         (float)Math.Sin(tangentYaw));
//                     var leftNormal = new Vector2(
//                         -tangent.Y,
//                         tangent.X);
//                     var positionError =
//                         currentPosition - referencePoint;
//                     var lateralErrorMeters =
//                         Vector2.Dot(
//                             positionError,
//                             leftNormal) / 1000.0;
//                     var normalCorrection =
//                         Limit(
//                             -LateralGainPerSecond *
//                             lateralErrorMeters,
//                             MaximumLateralCorrection);

//                     // 先在世界坐标中组合切向速度与横向纠偏速度。
//                     var worldVx =
//                         tangent.X * speed +
//                         leftNormal.X * (float)normalCorrection;
//                     var worldVy =
//                         tangent.Y * speed +
//                         leftNormal.Y * (float)normalCorrection;

//                     // 将世界速度表达为当前蟹行运动坐标系速度。
//                     var motionYaw =
//                         currentBodyYaw +
//                         MotionFrameYawInBodyRadians;
//                     var motionCos = Math.Cos(motionYaw);
//                     var motionSin = Math.Sin(motionYaw);
//                     var vxInMotion =
//                         motionCos * worldVx +
//                         motionSin * worldVy;
//                     var vyInMotion =
//                         -motionSin * worldVx +
//                         motionCos * worldVy;

//                     var desiredBodyYaw =
//                         tangentYaw -
//                         MotionFrameYawInBodyRadians;
//                     var headingError =
//                         AngleMath.ShortestDifferenceRadians(
//                             desiredBodyYaw,
//                             currentBodyYaw);
//                     var omega =
//                         speed * referenceCurvature +
//                         HeadingGainPerSecond * headingError;
//                     omega = Limit(
//                         omega,
//                         MaximumAngularSpeedRadiansPerSecond);

//                     var now = DateTime.Now;
//                     var interval = now - lastCommandTime;
//                     lastCommandTime = now;

//                     bool commandAccepted;
//                     Twist2D bodyTwist;
//                     if (CommandBackend ==
//                         ChassisCommandBackend.SendMotion)
//                     {
//                         // 运动坐标系相对车体系旋转+90°：
//                         // 运动系正向速度会转换成车体系+Y速度。
//                         bodyTwist =
//                             FrameTransform2D
//                                 .TransformTwistAtSamePoint(
//                                     new Pose2D(
//                                         0.0,
//                                         0.0,
//                                         MotionFrameYawInBodyRadians),
//                                     new Twist2D(
//                                         vxInMotion,
//                                         vyInMotion,
//                                         omega));

//                         // 将运动坐标系原点和前后几何控制点处的速度，
//                         // 转换为SendMotion需要的前后轴方向。
//                         var controlPointRadiusMeters =
//                             Math.Max(
//                                 chassis.ControlPointRadius /
//                                 1000.0,
//                                 0.001);
//                         var frontVelocityY =
//                             vyInMotion +
//                             omega *
//                             controlPointRadiusMeters;
//                         var rearVelocityY =
//                             vyInMotion -
//                             omega *
//                             controlPointRadiusMeters;
//                         var frontSteeringRadians =
//                             Math.Atan2(
//                                 frontVelocityY,
//                                 vxInMotion);
//                         var rearSteeringRadians =
//                             Math.Atan2(
//                                 rearVelocityY,
//                                 vxInMotion);

//                         // 蟹行测试绕过M层ManualControl并直接调用SendMotion，
//                         // 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
//                         if (IsCrabMotionFrame())
//                         {
//                             var geometryRatio =
//                                 adapter.HalfTrackWidthMeters /
//                                 adapter.HalfWheelBaseMeters;

//                             frontSteeringRadians =
//                                 ConvertToCrabSteering(
//                                     frontSteeringRadians,
//                                     geometryRatio);
//                             rearSteeringRadians =
//                                 ConvertToCrabSteering(
//                                     rearSteeringRadians,
//                                     geometryRatio);
//                         }

//                         var frontThetaDegrees =
//                             (float)AngleMath.RadiansToDegrees(
//                                 frontSteeringRadians);
//                         var rearThetaDegrees =
//                             (float)AngleMath.RadiansToDegrees(
//                                 rearSteeringRadians);
//                         var motionSpeed =
//                             (float)Math.Sqrt(
//                                 vxInMotion * vxInMotion +
//                                 vyInMotion * vyInMotion);

//                         commandAccepted =
//                             chassis.SendMotion(
//                                 motionSpeed,
//                                 frontThetaDegrees,
//                                 rearThetaDegrees,
//                                 interval);
//                     }
//                     else if (CommandBackend ==
//                              ChassisCommandBackend
//                                  .SendXYThSpeed)
//                     {
//                         // 安全XYTh后端根据舵角误差统一压低驱动轮速。
//                         bodyTwist =
//                             FrameTransform2D
//                                 .TransformTwistAtSamePoint(
//                                     new Pose2D(
//                                         0.0,
//                                         0.0,
//                                         MotionFrameYawInBodyRadians),
//                                     new Twist2D(
//                                         vxInMotion,
//                                         vyInMotion,
//                                         omega));
//                         var command = new ChassisCommand(
//                             PilotDefinition.Self.CarNum,
//                             bodyTwist);
//                         commandAccepted =
//                             adapter.Send(
//                                 command,
//                                 interval);
//                     }
//                     else
//                     {
//                         throw new InvalidOperationException(
//                             $"不支持的底盘命令后端：{CommandBackend}。");
//                     }

//                     if (!commandAccepted)
//                         throw new InvalidOperationException(
//                             "运动坐标系轨迹底盘解算失败：" +
//                             chassis
//                                 .LastMotionDecomposeFailureReason);

//                     CommandObserver?.Invoke(
//                         (float)bodyTwist.VxMetersPerSecond,
//                         (float)bodyTwist.VyMetersPerSecond,
//                         (float)bodyTwist
//                             .OmegaRadiansPerSecond);

//                     yield return true;
//                 }
//             }
//             finally
//             {
//                 adapter.StopImmediately();
//                 if (CommandBackend ==
//                     ChassisCommandBackend.SendMotion)
//                 {
//                     // 测试退出后恢复真实车体坐标系，避免影响后续测试。
//                     adapter.ResetToBodyFrame();
//                 }
//                 CommandObserver?.Invoke(0f, 0f, 0f);
//             }

//             yield return false;
//         }

//         // 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
//         private bool IsCrabMotionFrame()
//         {
//             return Math.Abs(
//                        AngleMath.ShortestDifferenceRadians(
//                            Math.PI / 2.0,
//                            MotionFrameYawInBodyRadians)) <
//                    1e-6;
//         }

//         // 按车体几何比例缩小蟹行转角。
//         // +90°运动坐标系已经完成方向映射，此处不能再次反号。
//         private double ConvertToCrabSteering(
//             double normalSteeringRadians,
//             double geometryRatio)
//         {
//             var crabSteeringRadians =
//                 Math.Atan(
//                     geometryRatio *
//                     Math.Tan(
//                         normalSteeringRadians));

//             return Limit(
//                 crabSteeringRadians,
//                 MaximumVirtualSteeringRadians);
//         }

//         // 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
//         private void CalculateReference(
//             Vector2 currentPosition,
//             out double tangentYaw,
//             out Vector2 referencePoint,
//             out float remainingMillimeters,
//             out double curvaturePerMeter)
//         {
//             var initialMotionYaw =
//                 InitialBodyYawRadians +
//                 MotionFrameYawInBodyRadians;

//             if (PathKind == ReferencePathKind.Straight)
//             {
//                 var tangent = new Vector2(
//                     (float)Math.Cos(initialMotionYaw),
//                     (float)Math.Sin(initialMotionYaw));
//                 var relative = currentPosition - StartPosition;
//                 var progress =
//                     Vector2.Dot(relative, tangent);
//                 var clampedProgress =
//                     Math.Max(
//                         0f,
//                         Math.Min(progress, LengthMillimeters));

//                 tangentYaw = initialMotionYaw;
//                 referencePoint =
//                     StartPosition +
//                     tangent * clampedProgress;
//                 remainingMillimeters =
//                     Math.Max(
//                         0f,
//                         LengthMillimeters - progress);
//                 curvaturePerMeter = 0.0;
//                 return;
//             }

//             if (PathKind == ReferencePathKind.SCurve)
//             {
//                 CalculateSCurveReference(
//                     currentPosition,
//                     initialMotionYaw,
//                     out tangentYaw,
//                     out referencePoint,
//                     out remainingMillimeters,
//                     out curvaturePerMeter);
//                 return;
//             }

//             var center = GetArcCenter();
//             var startRadialYaw =
//                 initialMotionYaw - Math.PI / 2.0;
//             var radial = currentPosition - center;
//             var currentRadialYaw =
//                 Math.Atan2(radial.Y, radial.X);
//             var progressRadians =
//                 AngleMath.NormalizeRadians(
//                     currentRadialYaw - startRadialYaw);

//             // 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
//             if (progressRadians < 0.0)
//                 progressRadians = 0.0;

//             var clampedProgressRadians =
//                 Math.Min(
//                     progressRadians,
//                     ArcSweepRadians);
//             var referenceRadialYaw =
//                 startRadialYaw +
//                 clampedProgressRadians;
//             referencePoint = center + new Vector2(
//                 RadiusMillimeters *
//                     (float)Math.Cos(referenceRadialYaw),
//                 RadiusMillimeters *
//                     (float)Math.Sin(referenceRadialYaw));
//             tangentYaw =
//                 referenceRadialYaw + Math.PI / 2.0;
//             remainingMillimeters =
//                 (float)Math.Max(
//                     0.0,
//                     (ArcSweepRadians - progressRadians) *
//                     RadiusMillimeters);
//             curvaturePerMeter =
//                 1000.0 / RadiusMillimeters;
//         }

//         // 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
//         private void CalculateSCurveReference(
//             Vector2 currentPosition,
//             double initialMotionYaw,
//             out double tangentYaw,
//             out Vector2 referencePoint,
//             out float remainingMillimeters,
//             out double curvaturePerMeter)
//         {
//             const int nearestPointSamples = 200;
//             var searchStart =
//                 Math.Max(
//                     0.0,
//                     _lastSCurveProgress - 0.02);
//             var bestProgress = _lastSCurveProgress;
//             var bestDistanceSquared = double.MaxValue;

//             for (var i = 0;
//                  i <= nearestPointSamples;
//                  i++)
//             {
//                 var progress =
//                     searchStart +
//                     (1.0 - searchStart) *
//                     i / nearestPointSamples;
//                 EvaluateSCurve(
//                     progress,
//                     out var localPoint,
//                     out _,
//                     out _);
//                 var worldPoint =
//                     LocalPathPointToWorld(
//                         localPoint,
//                         initialMotionYaw);
//                 var distanceSquared =
//                     Vector2.DistanceSquared(
//                         currentPosition,
//                         worldPoint);

//                 if (distanceSquared <
//                     bestDistanceSquared)
//                 {
//                     bestDistanceSquared =
//                         distanceSquared;
//                     bestProgress = progress;
//                 }
//             }

//             // 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
//             _lastSCurveProgress =
//                 Math.Max(
//                     _lastSCurveProgress,
//                     bestProgress);
//             EvaluateSCurve(
//                 _lastSCurveProgress,
//                 out var bestLocalPoint,
//                 out var firstDerivative,
//                 out var secondDerivative);
//             referencePoint =
//                 LocalPathPointToWorld(
//                     bestLocalPoint,
//                     initialMotionYaw);
//             tangentYaw =
//                 initialMotionYaw +
//                 Math.Atan2(
//                     firstDerivative.Y,
//                     firstDerivative.X);

//             var derivativeMagnitude =
//                 Math.Sqrt(
//                     firstDerivative.X *
//                     firstDerivative.X +
//                     firstDerivative.Y *
//                     firstDerivative.Y);
//             if (derivativeMagnitude < 1e-6)
//             {
//                 curvaturePerMeter = 0.0;
//             }
//             else
//             {
//                 // 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
//                 curvaturePerMeter =
//                     (firstDerivative.X *
//                      secondDerivative.Y -
//                      firstDerivative.Y *
//                      secondDerivative.X) *
//                     1000.0 /
//                     Math.Pow(
//                         derivativeMagnitude,
//                         3.0);
//             }

//             remainingMillimeters =
//                 ApproximateSCurveRemainingLength(
//                     _lastSCurveProgress);
//         }

//         // 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
//         private void EvaluateSCurve(
//             double progress,
//             out Vector2 point,
//             out Vector2 firstDerivative,
//             out Vector2 secondDerivative)
//         {
//             progress =
//                 Math.Max(
//                     0.0,
//                     Math.Min(progress, 1.0));

//             Vector2 p0;
//             Vector2 p1;
//             Vector2 p2;
//             Vector2 p3;
//             double t;

//             if (progress <= 0.25)
//             {
//                 t = progress * 4.0;
//                 p0 = new Vector2(0f, 0f);
//                 p1 = new Vector2(
//                     LengthMillimeters / 12f,
//                     0f);
//                 p2 = new Vector2(
//                     LengthMillimeters / 6f,
//                     SCurveLateralOffsetMillimeters);
//                 p3 = new Vector2(
//                     LengthMillimeters * 0.25f,
//                     SCurveLateralOffsetMillimeters);
//             }
//             else if (progress <= 0.75)
//             {
//                 t = (progress - 0.25) * 2.0;
//                 p0 = new Vector2(
//                     LengthMillimeters * 0.25f,
//                     SCurveLateralOffsetMillimeters);
//                 p1 = new Vector2(
//                     LengthMillimeters / 3f,
//                     SCurveLateralOffsetMillimeters);
//                 p2 = new Vector2(
//                     LengthMillimeters * 2f / 3f,
//                     -SCurveLateralOffsetMillimeters);
//                 p3 = new Vector2(
//                     LengthMillimeters * 0.75f,
//                     -SCurveLateralOffsetMillimeters);
//             }
//             else
//             {
//                 t = (progress - 0.75) * 4.0;
//                 p0 = new Vector2(
//                     LengthMillimeters * 0.75f,
//                     -SCurveLateralOffsetMillimeters);
//                 p1 = new Vector2(
//                     LengthMillimeters * 5f / 6f,
//                     -SCurveLateralOffsetMillimeters);
//                 p2 = new Vector2(
//                     LengthMillimeters * 11f / 12f,
//                     0f);
//                 p3 = new Vector2(
//                     LengthMillimeters,
//                     0f);
//             }

//             var oneMinusT = 1.0 - t;
//             point =
//                 p0 * (float)(
//                     oneMinusT *
//                     oneMinusT *
//                     oneMinusT) +
//                 p1 * (float)(
//                     3.0 *
//                     oneMinusT *
//                     oneMinusT *
//                     t) +
//                 p2 * (float)(
//                     3.0 *
//                     oneMinusT *
//                     t *
//                     t) +
//                 p3 * (float)(t * t * t);
//             firstDerivative =
//                 (p1 - p0) *
//                     (float)(
//                         3.0 *
//                         oneMinusT *
//                         oneMinusT) +
//                 (p2 - p1) *
//                     (float)(
//                         6.0 *
//                         oneMinusT *
//                         t) +
//                 (p3 - p2) *
//                     (float)(3.0 * t * t);
//             secondDerivative =
//                 (p2 - 2f * p1 + p0) *
//                     (float)(6.0 * oneMinusT) +
//                 (p3 - 2f * p2 + p1) *
//                     (float)(6.0 * t);
//         }

//         // 通过分段采样估算从当前S曲线进度到终点的实际弧长。
//         private float ApproximateSCurveRemainingLength(
//             double startProgress)
//         {
//             const int lengthSamples = 100;
//             EvaluateSCurve(
//                 startProgress,
//                 out var previousPoint,
//                 out _,
//                 out _);
//             var length = 0f;

//             for (var i = 1;
//                  i <= lengthSamples;
//                  i++)
//             {
//                 var progress =
//                     startProgress +
//                     (1.0 - startProgress) *
//                     i / lengthSamples;
//                 EvaluateSCurve(
//                     progress,
//                     out var point,
//                     out _,
//                     out _);
//                 length +=
//                     Vector2.Distance(
//                         previousPoint,
//                         point);
//                 previousPoint = point;
//             }

//             return length;
//         }

//         // 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
//         private Vector2 LocalPathPointToWorld(
//             Vector2 localPoint,
//             double initialMotionYaw)
//         {
//             var cos =
//                 (float)Math.Cos(initialMotionYaw);
//             var sin =
//                 (float)Math.Sin(initialMotionYaw);

//             return StartPosition + new Vector2(
//                 localPoint.X * cos -
//                 localPoint.Y * sin,
//                 localPoint.X * sin +
//                 localPoint.Y * cos);
//         }

//         // 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
//         public Vector2 GetArcCenter()
//         {
//             var initialMotionYaw =
//                 InitialBodyYawRadians +
//                 MotionFrameYawInBodyRadians;
//             return StartPosition + new Vector2(
//                 -RadiusMillimeters *
//                     (float)Math.Sin(initialMotionYaw),
//                 RadiusMillimeters *
//                     (float)Math.Cos(initialMotionYaw));
//         }

//         // 获取圆弧测试的理论终点。
//         public Vector2 GetArcDestination()
//         {
//             var initialMotionYaw =
//                 InitialBodyYawRadians +
//                 MotionFrameYawInBodyRadians;
//             var startRadialYaw =
//                 initialMotionYaw - Math.PI / 2.0;
//             var endRadialYaw =
//                 startRadialYaw + ArcSweepRadians;
//             var center = GetArcCenter();

//             return center + new Vector2(
//                 RadiusMillimeters *
//                     (float)Math.Cos(endRadialYaw),
//                 RadiusMillimeters *
//                     (float)Math.Sin(endRadialYaw));
//         }

//         // 根据剩余路径长度生成终点减速速度。
//         private float CalculateSpeed(
//             float remainingMillimeters)
//         {
//             if (remainingMillimeters >=
//                 SlowDistanceMillimeters)
//                 return CruiseSpeed;

//             var ratio =
//                 remainingMillimeters /
//                 Math.Max(
//                     SlowDistanceMillimeters,
//                     1f);
//             return Math.Max(
//                 MinimumSpeed,
//                 CruiseSpeed * ratio);
//         }

//         private void ValidateParameters()
//         {
//             if (CruiseSpeed <= 0f ||
//                 !IsFinite(CruiseSpeed) ||
//                 LengthMillimeters <= 0f ||
//                 !IsFinite(LengthMillimeters) ||
//                 RadiusMillimeters <= 0f ||
//                 !IsFinite(RadiusMillimeters) ||
//                 SCurveLateralOffsetMillimeters <= 0f ||
//                 !IsFinite(
//                     SCurveLateralOffsetMillimeters) ||
//                 ArcSweepRadians <= 0.0 ||
//                 !IsFinite(ArcSweepRadians) ||
//                 SlowDistanceMillimeters <= 0f ||
//                 !IsFinite(SlowDistanceMillimeters) ||
//                 FinishDistanceMillimeters < 0f ||
//                 !IsFinite(FinishDistanceMillimeters) ||
//                  TrackingTimeoutSeconds <= 0f ||
//                  !IsFinite(TrackingTimeoutSeconds) ||
//                  MaximumVirtualSteeringRadians <= 0.0 ||
//                  MaximumVirtualSteeringRadians >=
//                  Math.PI / 2.0 ||
//                  !IsFinite(
//                      MaximumVirtualSteeringRadians))
//                 throw new ArgumentOutOfRangeException(
//                     "蟹行轨迹测试参数无效。");
//         }

//         private static double Limit(
//             double value,
//             double absoluteLimit)
//         {
//             return Math.Max(
//                 -absoluteLimit,
//                 Math.Min(value, absoluteLimit));
//         }

//         private static bool IsFinite(double value)
//         {
//             return
//                 !double.IsNaN(value) &&
//                 !double.IsInfinity(value);
//         }
//     }
// }