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

// namespace MultiWheelC
// {
//     // C层单车底盘：按照车轮里程行驶指定的相对距离。
//     public class LineTracking : MovementDefinition
//     {
//         // 相对动作启动位置的行驶距离，单位mm。
//         // 正数表示前进，负数表示后退。
//         public float TargetDistance;
//         public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
//         public float Kp = PilotDefinition.Conf.LineTrackKp;
//         public float Ki = PilotDefinition.Conf.LineTrackKi;
//         public float Kd = PilotDefinition.Conf.LineTrackKd;
//         public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
//         public int SrcId = -1;
//         public int DstId = -1;
//         public Action<int> LeaveSrcFunction;
//         // 接近目标后是否保留速度，交给下一个动作接管。
//         public bool EnableHandover;
//         // 进入动作衔接的剩余距离，单位mm。
//         public float HandoverDistance = 80f;
//         // HandoverSpeed小于0时，使用MaxSpeed的此比例。
//         public float HandoverSpeedRatio = 0.5f;
//         // 大于等于0时，直接作为衔接速度，单位m/s。
//         public float HandoverSpeed = -1f;
//         public float MinHandoverSpeed = 0.05f;
//         private PIDController _pid;
//         // 读取当前单车直线行驶里程，单位mm。
//         private static float ReadPosition()
//         {
//             return
//                 (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
//         }

//         // 根据动作启动位置和目标距离执行直线里程闭环。
//         public override IEnumerable<bool> Get()
//         {
//             if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
//             {
//                 throw new ArgumentOutOfRangeException(
//                     nameof(TargetDistance),
//                     "目标行驶距离必须是有限值。");
//             }

//             if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
//             {
//                 throw new ArgumentOutOfRangeException(
//                     nameof(MaxSpeed),
//                     "最大速度必须是大于零的有限值。");
//             }
//             var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
//             // 每次启动动作时重新读取起始编码器位置。
//             var startPosition = ReadPosition();
//             // PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
//             var targetPosition = startPosition + TargetDistance;
//             _pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
//             {
//                 SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
//             };
//             var handoverRequested = false;
//             var keepHandoverSpeed = false;
//             DLog.Log(
//                 $"直线里程动作：" +
//                 $"起点={startPosition:F1}mm，" +
//                 $"距离={TargetDistance:F1}mm，" +
//                 $"目标={targetPosition:F1}mm",
//                 "straight_line");
//             try
//             {
//                 while (true)
//                 {
//                     var currentPosition = ReadPosition();
//                     var remainingDistance = targetPosition - currentPosition;
//                     // 接近目标后，保留一定速度交给后续动作。
//                     if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
//                     {
//                         var direction = Math.Sign(remainingDistance);
//                         if (direction == 0)
//                         {
//                             direction = Math.Sign(TargetDistance);
//                         }
//                         var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
//                         var maximumSpeed = Math.Abs(MaxSpeed);
//                         var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
//                         var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
//                         var handoverSpeed = limitedSpeed * direction;
//                         chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
//                         handoverRequested = true;
//                         // 保持一个调度周期，让速度命令实际生效。
//                         yield return true;
//                         break;
//                     }
//                     var speed = _pid.GetResponse(targetPosition);
//                     chassis.SendXYThSpeed(speed, 0f, 0f);
//                     if (_pid.IsArrived())
//                     {
//                         break;
//                     }
//                     yield return true;
//                 }
//                 if (SrcId != -1 &&
//                     LeaveSrcFunction != null)
//                 {
//                     LeaveSrcFunction(SrcId);
//                     DLog.Log($"释放放车点{SrcId}", "straight_line");
//                 }
//                 // 只有正常完成动作衔接时才允许保留非零速度。
//                 keepHandoverSpeed = handoverRequested;
//             }
//             finally
//             {
//                 // 普通完成、人工停止或异常退出时都必须停车。
//                 if (!keepHandoverSpeed)
//                 {
//                     chassis.SendXYThSpeed(0f, 0f, 0f);
//                 }
//             }
//             yield return false;
//         }
//     }




//     //直线行走基于detour
//     public class LineTracking_based_detour : MovementDefinition
//     {
//         public float LineDistance = 1000f;
//         public int SrcId = -1;
//         public int DstId = -1;
//         public Action<int> LeaveSrcFunction = null;
//         public Painter painter = UI.GetPainter("Line", false);
//         // C层单车轨迹：执行早期版本的两点直线跟踪动作。
//         public override IEnumerable<bool> Get()
//         {
//             var curpose = DetourInterface.getCartLocation();
//             Console.WriteLine($"curpose.th:{curpose.th}");
//             var src = new Vector2((float)curpose.x, (float)curpose.y);
//             var headingRadians =
//                 AngleMath.DegreesToRadians(curpose.th);
//             var dst = new Vector2(
//                 (float)(curpose.x +
//                     LineDistance * Math.Cos(headingRadians)),
//                 (float)(curpose.y +
//                     LineDistance * Math.Sin(headingRadians)));
//             // var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
//             //                       (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
//             Console.WriteLine($"src:{src.X} {src.Y}");
//             Console.WriteLine($"dst:{dst.X} {dst.Y}");
//             painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);

//             var tracker = new ChassisController().Get();
//             var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
//             tracker.AddTrack(linePath);
//             var _dt = new DriveTask(tracker.Track());
//             _dt.Wait();
//             if (SrcId != -1 && LeaveSrcFunction != null)
//             {
//                 LeaveSrcFunction(SrcId);
//                 DLog.Log($"释放放车点{SrcId}", "straight_line");
//             }
//             yield return false;
//         }
//     }
// }