using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Diagnostics;
using System.Text;
using CommonUsage.Mathematics;
using FundamentalLib;
using System.Drawing;

namespace CommonUsage.Chassis
{
    public class MultiWheelChassis : AbstractChassis
    {
        public float ThConsistentThreshold = 1.0f;

        public float ControlPointRadius = 500f;

        // 原地旋转纠偏钳位：SendRotateMotion 中每轮纠偏速度幅值不超过 该比例×本轮旋转切向速度，
        // 防止减速末段旋转切向变小时纠偏盖过它、使轮向矢量乱摆（频繁打方向/卡死）。<0 关闭钳位。
        public float RotateCompTangentFrac = 0.5f;

        // 上一次 SendRotateMotion 的舵轮对齐状态：false 表示舵轮未追上目标角(gate=0、车未真正转动)。
        // 供上层做积分抗饱和（卡死时冻结积分）。
        public bool LastRotateAligned { get; private set; } = true;

        public string LastMotionDecomposeFailureReason { get; private set; } = "";

        // 为true时，在两个等价舵角都可用的情况下优先选择机械转动距离更小的方案。
        // 默认关闭以保持旧项目原有行为，由需要该策略的车型适配器主动开启。
        public bool PreferMinimumSteeringTravel { get; set; }

        public float MinimumTurningAngleForAckermann = 60f;


        public MultiWheelChassis() : base()
        {

        }

        public void AddWheel(SteerWheel wheel)
        {
            _steerWheels.Add(wheel);
        }

        public override void Initialize()
        {
            _steerWheels = _steerWheels.OrderByDescending(sw => sw.Position.X).ToList();
            _targetSpeeds = new float[_steerWheels.Count].ToList();
            _sendSpeeds = new float[_steerWheels.Count].ToList();
            _tmpSpeeds = new float[_steerWheels.Count].ToList();
            _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList();
            _sendAngle = new float[_steerWheels.Count].ToList();
            _debugSpeeds = new float[_steerWheels.Count].ToList();

            if (_steerWheels.Count < 2)
            {
                Console.WriteLine($"steer wheel num: {_steerWheels.Count}. invalid!");
                Valid = false;
                return;
            }

            CalculateAxes();
            Valid = true;
        }

        public Visualizer MainVisualizer = null;

        public override void Visualize()
        {
            MainVisualizer?.Clear();

            if (_rotCenter != Vector2.Zero)
            {
                MainVisualizer?.DrawText(Color.Chartreuse, $"({_rotCenter.X:f1}, {_rotCenter.Y:f2})", _rotCenter);
            }

            for (var i = 0; i < _steerWheels.Count; ++i)
            {
                var sw = _steerWheels[i];
                if (sw is DiffSteerWheel dsw)
                {
                    var pointing = CommonMath.Transform2D(dsw.Position, dsw.ZeroDirection, new Vector2(200, 0));
                    MainVisualizer?.DrawLine(Color.Gray, dsw.Position, pointing);
                    MainVisualizer?.DrawText(Color.Gray, $"{dsw.GetSendSpeed():f2} {sw.GetSendAngle():f2}", dsw.Position);

                    var left = CommonMath.Transform2D(dsw.Position, dsw.GetAngleRelativeToChassis() + 90, new Vector2(dsw.WheelDistance, 0));
                    MainVisualizer?.DrawLine(Color.GreenYellow, left,
                        CommonMath.Transform2D(left, dsw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true);
                    MainVisualizer?.DrawText(Color.GreenYellow, $"{dsw.GetLeftSendSpeed():0.00}", left);

                    var right = CommonMath.Transform2D(dsw.Position, dsw.GetAngleRelativeToChassis() - 90, new Vector2(dsw.WheelDistance, 0));
                    MainVisualizer?.DrawLine(Color.DeepPink, right,
                        CommonMath.Transform2D(right, dsw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true);
                    MainVisualizer?.DrawText(Color.DeepPink, $"{dsw.GetRightSendSpeed():0.00}", right);
                }
                else
                {
                    MainVisualizer?.DrawLine(Color.Green, sw.Position,
                        CommonMath.Transform2D(sw.Position, sw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true);
                    MainVisualizer?.DrawText(Color.Chartreuse, $"{sw.GetSendSpeed():f2} {sw.GetAngleRelativeToChassis():f2}", sw.Position);
                }

                if (_rotCenter != Vector2.Zero)
                {
                    MainVisualizer?.DrawLine(Color.DodgerBlue, sw.Position, _rotCenter);
                }
            }
        }

        public override void AfterDirectionChanged()
        {
            Hedingben.ToastText($"AfterDirectionChanged", "MultiWheelChassis-AfterDirectionChanged");

            foreach (var sw in _steerWheels)
            {
                sw.Position = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh,
                    sw.PhysicalPosition);
                sw.ZeroDirection = _originBiasTh;
            }

            CalculateAxes();
            GoingWheelAligned = false;
        }

        protected override void DefineGeometricWheelComputation(float speed)
        {
            if (!GoingActive) ResetMotionState();
            var targetGcpTheta0 = GeometricControlPoints[0].Theta;
            var targetGcpTheta1 = GeometricControlPoints[1].Theta;
            var lastGcpTheta0Before = _lastGcpTheta0;
            var lastGcpTheta1Before = _lastGcpTheta1;

            void Process(ref float current, float target, int id)
            {
                current += Math.Sign(target - current) * Math.Min(Math.Abs(target - current),
                    GcpThetaPerSecond * (float)(DateTime.Now - LastMoveTime).TotalSeconds);
                // Hedingben.ToastText($"current:{current:0.00} target:{target:0.00}", $"gcp-accumulation-{id}");
            }

            Process(ref _lastGcpTheta0, GeometricControlPoints[0].Theta, 0);
            Process(ref _lastGcpTheta1, GeometricControlPoints[1].Theta, 1);
            if ((DateTime.Now - _geometricComputeLastLog).TotalMilliseconds >= 200)
            {
                _geometricComputeLastLog = DateTime.Now;
                DLog.Log(
                    $"ComputeWheelsGeometrically speed:{speed:F3} " +
                    $"targetGcp:({targetGcpTheta0:F1},{targetGcpTheta1:F1}) " +
                    $"smoothBefore:({lastGcpTheta0Before:F1},{lastGcpTheta1Before:F1}) " +
                    $"smoothAfter:({_lastGcpTheta0:F1},{_lastGcpTheta1:F1}) " +
                    $"originBias:({_originBiasX:F1},{_originBiasY:F1},{_originBiasTh:F1}) goingActive:{GoingActive}",
                    "MultiWheelGoDiag");
            }
            SendMotion(speed, _lastGcpTheta0, _lastGcpTheta1);

            GoingActive = true;
            RotatingActive = false;
            XYThActive = false;
        }

        private float _lastGcpTheta0;

        private float _lastGcpTheta1;

        /// <summary>
        /// 转速单位为度/s，逆时针为正。
        /// </summary>
        /// <param name="rotSpeed"></param>
        public override bool ComputeRotateWheels(float rotSpeed)
        {
            if (!RotatingActive) ResetMotionState();

            var feasible = SendRotateMotion(rotSpeed);

            GoingActive = false;
            RotatingActive = true;
            XYThActive = false;
            return feasible;
        }

        public override void PredefinedDriveStop()
        {
            if (!Valid) return;
            for (var i = 0; i < _steerWheels.Count; i++)
            {
                _sendSpeeds[i] = 0;
                _debugSpeeds[i] = 0;
                if (_steerWheels[i] is DiffSteerWheel dfw)
                {
                    dfw.WriteLeftSpeed(0);
                    dfw.WriteRightSpeed(0);
                }
                else
                {
                    _steerWheels[i].WriteSpeed(0);
                }
            }
            GoingActive = false;
            RotatingActive = false;
            XYThActive = false;
        }

        public void RampStop(TimeSpan? deltaTime = null)
        {
            if (!Valid) return;

            for (var i = 0; i < _steerWheels.Count; i++)
            {
                _debugSpeeds[i] = 0;
                AccumulateSpeed(i, 0, false, Vector2.Zero, deltaTime);
            }

            GoingActive = false;
            RotatingActive = false;
            XYThActive = false;
            LastMoveTime = DateTime.Now;
        }

        /// <summary>
        /// 立即清零XYTh驱动轮速度，同时保留已经准备好的舵角目标和轮速方向。
        /// 下一条非零命令仍需重新确认四轮实际舵角到位后才会开放驱动速度。
        /// </summary>
        public void StopXYThDrivePreserveSteeringState()
        {
            if (!Valid) return;

            // 只有已完成Prepare/Adopt交接的XYTh模式才能保留状态。
            if (!XYThActive)
            {
                PredefinedDriveStop();
                return;
            }

            for (var i = 0; i < _steerWheels.Count; i++)
            {
                _targetSpeeds[i] = 0;
                _sendSpeeds[i] = 0;
                _debugSpeeds[i] = 0;

                if (_steerWheels[i] is DiffSteerWheel diffSteerWheel)
                {
                    diffSteerWheel.WriteLeftSpeed(0);
                    diffSteerWheel.WriteRightSpeed(0);
                }
                else
                {
                    _steerWheels[i].WriteSpeed(0);
                }
            }

            GoingActive = false;
            RotatingActive = false;

            // 保留XYThActive、_sendAngle和_wheelDirs，避免重新选择等价舵角；
            // 清除到位标记，使下一次推动摇杆时重新核对实际反馈。
            _xyThWheelsAligned = false;
            LastMoveTime = DateTime.Now;
            LastMotionDecomposeFailureReason = "";
        }

        private struct WheelAngleCandidate
        {
            public bool Valid;
            public float RawAngle;
            public float NormalizedAngle;
            public float Margin;
            public int Direction;
            public float Lower;
            public float Upper;
        }

        private WheelAngleCandidate BuildWheelAngleCandidate(SteerWheel sw, float rawAngle, int direction)
        {
            var lower = (float)CommonMath.RoundTh(sw.AngleLowerLimit);
            var upper = (float)CommonMath.RoundTh(sw.AngleUpperLimit);
            while (upper < lower) upper += 360;

            var normalized = (float)CommonMath.RoundTh(rawAngle);
            while (normalized < lower) normalized += 360;
            while (normalized > upper && normalized - 360 >= lower) normalized -= 360;

            var margin = Math.Min(normalized - lower, upper - normalized);
            var marginRequired = Math.Max(0, sw.AngleLimitMarginDeg);
            return new WheelAngleCandidate
            {
                Valid = normalized >= lower && normalized <= upper && margin >= marginRequired,
                RawAngle = rawAngle,
                NormalizedAngle = normalized,
                Margin = margin,
                Direction = direction,
                Lower = lower,
                Upper = upper
            };
        }

        private bool TryResolveWheelAngle(int wheelIndex, float targetAngle, string entry,
            out float angle, out int direction, out string reason)
        {
            var sw = _steerWheels[wheelIndex];
            var forward = BuildWheelAngleCandidate(sw, targetAngle, 1);
            var reverse = BuildWheelAngleCandidate(sw, targetAngle + 180, -1);

            WheelAngleCandidate? selected = null;

            if (PreferMinimumSteeringTravel &&
                forward.Valid &&
                reverse.Valid)
            {
                var actualAngle = sw.ReadAngle();

                if (!float.IsNaN(actualAngle) &&
                    !float.IsInfinity(actualAngle))
                {
                    // 将反馈角映射到与候选角相同的机械限位区间。
                    // 这里比较的是受限机械舵角，不能使用圆周最短角度差。
                    var actual = BuildWheelAngleCandidate(
                        sw,
                        actualAngle,
                        1);
                    var forwardError = Math.Abs(
                        forward.NormalizedAngle -
                        actual.NormalizedAngle);
                    var reverseError = Math.Abs(
                        reverse.NormalizedAngle -
                        actual.NormalizedAngle);

                    // 迟滞避免两个方案转动量接近时频繁翻转轮速方向。
                    const float switchHysteresisDegrees = 5f;

                    if (reverseError +
                        switchHysteresisDegrees <
                        forwardError)
                    {
                        selected = reverse;
                    }
                    else if (forwardError +
                             switchHysteresisDegrees <
                             reverseError)
                    {
                        selected = forward;
                    }
                }
            }

            // 未启用最小转舵策略、反馈无效或两个候选差异落在迟滞区时，
            // 保持旧版轮速方向，避免破坏其他车型的既有行为。
            if (_wheelDirs != null && wheelIndex < _wheelDirs.Count)
            {
                if (selected == null &&
                    _wheelDirs[wheelIndex] < 0 &&
                    reverse.Valid)
                {
                    selected = reverse;
                }
                else if (selected == null &&
                         _wheelDirs[wheelIndex] >= 0 &&
                         forward.Valid)
                {
                    selected = forward;
                }
            }

            if (selected == null)
            {
                if (forward.Valid) selected = forward;
                else if (reverse.Valid) selected = reverse;
            }

            if (selected != null)
            {
                angle = selected.Value.NormalizedAngle;
                direction = selected.Value.Direction;
                reason = "";
                return true;
            }

            angle = 0;
            direction = 0;
            var marginRequired = Math.Max(0, sw.AngleLimitMarginDeg);
            reason =
                $"{entry} infeasible wheel={wheelIndex} target={CommonMath.RoundTh(targetAngle):F1} " +
                $"reverse={CommonMath.RoundTh(targetAngle + 180):F1} " +
                $"limit=[{forward.Lower:F1},{forward.Upper:F1}] requiredMargin={marginRequired:F1} " +
                $"forward(norm={forward.NormalizedAngle:F1},margin={forward.Margin:F1},valid={forward.Valid}) " +
                $"reverse(norm={reverse.NormalizedAngle:F1},margin={reverse.Margin:F1},valid={reverse.Valid})";
            return false;
        }

        private bool FailMotionDecomposition(string entry, string reason, TimeSpan? deltaTime)
        {
            LastMotionDecomposeFailureReason = $"{entry}: {reason}";
            Console.WriteLine(LastMotionDecomposeFailureReason);
            DLog.Log(LastMotionDecomposeFailureReason, "MultiWheelMotionDecompose");
            Hedingben.ToastText($"Motion decompose failed: {reason}", "MultiWheelChassis-decompose");
            RampStop(deltaTime);
            return false;
        }

        private Vector2 _rotCenter;

        public Vector2 GetRotCenter()
        {
            return _rotCenter;
        }

        public void SetOriginBias(float x, float y, float th)
        {
            _originBiasX = x;
            _originBiasY = y;
            _originBiasTh = th;

            foreach (var sw in _steerWheels)
            {
                sw.Position = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh,
                    sw.PhysicalPosition);
                sw.ZeroDirection = _originBiasTh;
            }

            CalculateAxes();
        }

        public Visualizer SendMotionVisualizer = null;

        /// <summary>
        /// frontTh和rearTh是基于当前行进方向，前后轴应该打的角度。
        /// </summary>
        /// <param name="speed">单位m/s。</param>
        /// <param name="frontTh">角度制。基于当前行进方向。</param>
        /// <param name="rearTh">角度制。基于当前行进方向。</param>
        /// <param name="deltaTime">上次下发SendMotion到本次的间隔时间。</param>
        /// <param name="localControlRadius">用于多车联动，大于0时生效。</param>
        /// <param name="localCompensateX">用于多车联动，localControlRadius大于0时生效。</param>
        /// <param name="localCompensateY">用于多车联动，localControlRadius大于0时生效。</param>
        /// <param name="localCompensateTh">用于多车联动，localControlRadius大于0时生效。</param>
        /// <exception cref="Exception"></exception>
        public bool SendMotion(float speed, float frontTh, float rearTh, TimeSpan? deltaTime = null,
            float localControlRadius = 0, float localCompensateX = 0f, float localCompensateY = 0, float localCompensateTh = 0)
        {
            if (!Valid) return FailMotionDecomposition("SendMotion", "invalid chassis", deltaTime);
            LastMotionDecomposeFailureReason = "";

            var compInputSpeed = speed;
            var compInputFrontTh = frontTh;
            var compInputRearTh = rearTh;
            var compMotionTh = 0f;
            var compAlong = 0f;
            var compSide = 0f;
            var compDSpeed = 0f;
            var compDTh = 0f;

            if (localControlRadius > 0)
            {
                var motionTh = AverageAngle(frontTh, rearTh);
                var radMotionTh = motionTh / 180f * Math.PI;
                var alongComp = (float)(localCompensateX * Math.Cos(radMotionTh) +
                                        localCompensateY * Math.Sin(radMotionTh));
                var sideComp = (float)(-localCompensateX * Math.Sin(radMotionTh) +
                                       localCompensateY * Math.Cos(radMotionTh));
                var dSpeed = alongComp / 1000f;
                speed += dSpeed;

                var speedSign = Math.Abs(compInputSpeed) > 1e-4f ? Math.Sign(compInputSpeed) : 1;
                var steerBaseSpeed = Math.Max(Math.Abs(speed), 0.05f);
                var dTh = (float)(Math.Atan2(sideComp / 1000f, steerBaseSpeed) / Math.PI * 180f) * speedSign;
                compMotionTh = motionTh;
                compAlong = alongComp;
                compSide = sideComp;
                compDSpeed = dSpeed;
                compDTh = dTh;
                Hedingben.ToastText($"s:{compInputSpeed:F2} d:{dSpeed:F3}", "MultiWheelChassis-dSpeed");
                Hedingben.ToastText($"F:{frontTh:F1} R:{rearTh:F1} d:{dTh:F1}", "MultiWheelChassis-dTh");
                frontTh += dTh;
                rearTh += dTh;

                if (Math.Abs(localCompensateX) > 1e-3f || Math.Abs(localCompensateY) > 1e-3f ||
                    Math.Abs(localCompensateTh) > 1e-3f)
                    DLog.Log(
                        $"SendMotionComp in speed:{compInputSpeed:F3} fTh:{compInputFrontTh:F1} rTh:{compInputRearTh:F1} " +
                        $"motionTh:{motionTh:F1} biasTh:{_originBiasTh:F1} comp({localCompensateX:F1},{localCompensateY:F1},{localCompensateTh:F2}) " +
                        $"along:{alongComp:F1} side:{sideComp:F1} dSpeed:{dSpeed:F3} dTh:{dTh:F1} " +
                        $"out speed:{speed:F3} fTh:{frontTh:F1} rTh:{rearTh:F1}",
                        "MultiWheelMotionComp");
            }

            float scaleFactor = 1.0f;
            float currentFrontTh = frontTh;
            float currentRearTh = rearTh;
            bool needAdjust = true;
            const float minScaleFactor = 0.1f;
            var finalAngles = new List<float>();

            var axisDiffFlag = false;
            var myRotCenter = new Vector2();
            var lastInvalidReason = "";

            float AverageAngle(float left, float right)
            {
                var leftRad = left / 180f * Math.PI;
                var rightRad = right / 180f * Math.PI;
                var x = Math.Cos(leftRad) + Math.Cos(rightRad);
                var y = Math.Sin(leftRad) + Math.Sin(rightRad);
                if (Math.Abs(x) < 1e-6 && Math.Abs(y) < 1e-6)
                    return (float)CommonMath.RoundTh(left);
                return (float)CommonMath.RoundTh(Math.Atan2(y, x) / Math.PI * 180f);
            }

            float Interpolate(float left, float current, float right)
            {
                if (current <= left) return 0;
                if (current >= right) return 1;
                return (current - left) / (right - left);
            }

            while (needAdjust && scaleFactor > minScaleFactor)
            {
                needAdjust = false;
                var tmpAngles = new List<float>();

                // ... 现有的插值函数和旋转中心计算代码 ...
                var thDiff = CommonMath.ThDiff(currentFrontTh, currentRearTh);
                axisDiffFlag = Math.Abs(thDiff) > ThConsistentThreshold;

                var sign = -1;
                if (axisDiffFlag)
                {
                    // Vector2 pFront = new(_frontBase, 0), pRear = new(_rearBase, 0),
                    Vector2 pFront = new(ControlPointRadius, 0), pRear = new(-ControlPointRadius, 0),
                        normFront = CommonMath.Transform2D(pFront, currentFrontTh + 90, Vector2.UnitX),
                        normRear = CommonMath.Transform2D(pRear, currentRearTh + 90, Vector2.UnitX);
                    var (intersect, center) =
                        CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
                    if (!intersect)
                        return FailMotionDecomposition("SendMotion",
                            $"front/rear axes do not intersect frontTh={currentFrontTh:F1} rearTh={currentRearTh:F1}",
                            deltaTime);
                    myRotCenter = center;
                    sign = myRotCenter.Y > 1 ? 1 : -1;

                    if (localControlRadius > 0 && localCompensateTh != 0)
                    {
                        void VisPointing(Color color, Vector2 point, float th, int width)
                        {
                            var localFrontPointing = CommonMath.Transform2D(point, th, new Vector2(200, 0));
                            SendMotionVisualizer?.DrawLine(color, point, localFrontPointing, width: width);
                        }

                        var localPFront = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh,
                            new Vector2(localControlRadius, 0));
                        var localPRear = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh,
                            new Vector2(-localControlRadius, 0));
                        // SendMotionVisualizer?.DrawLine(Color.DarkRed, myRotCenter, localPFront);
                        // SendMotionVisualizer?.DrawLine(Color.DarkRed, myRotCenter, localPRear);

                        // 计算_rotCenter在"localPRear指向localPFront的向量"的左侧还是右侧
                        // 使用叉积判断：叉积 > 0 表示在左侧，< 0 表示在右侧
                        var vecRearToFront = new Vector2(localPFront.X - localPRear.X, localPFront.Y - localPRear.Y);
                        var vecRearToCenter = new Vector2(myRotCenter.X - localPRear.X, myRotCenter.Y - localPRear.Y);
                        var crossProduct = vecRearToFront.X * vecRearToCenter.Y - vecRearToFront.Y * vecRearToCenter.X;
                        var side = crossProduct > 0 ? 1 : -1;

                        var localFrontTh =
                            (float)(Math.Atan2(localPFront.Y - myRotCenter.Y, localPFront.X - myRotCenter.X) / Math.PI *
                                    180) + 90 * side;
                        var localRearTh =
                            (float)(Math.Atan2(localPRear.Y - myRotCenter.Y, localPRear.X - myRotCenter.X) / Math.PI *
                                    180) + 90 * side;
                        var sameDirectionError =
                            Math.Abs(CommonMath.ThDiff(localFrontTh, currentFrontTh)) +
                            Math.Abs(CommonMath.ThDiff(localRearTh, currentRearTh));
                        var reverseDirectionError =
                            Math.Abs(CommonMath.ThDiff(localFrontTh + 180, currentFrontTh)) +
                            Math.Abs(CommonMath.ThDiff(localRearTh + 180, currentRearTh));
                        if (reverseDirectionError < sameDirectionError)
                        {
                            localFrontTh = (float)CommonMath.RoundTh(localFrontTh + 180);
                            localRearTh = (float)CommonMath.RoundTh(localRearTh + 180);
                        }
                        // VisPointing(Color.DarkRed, localPFront, localFrontTh, 1);
                        // VisPointing(Color.DarkRed, localPRear, localRearTh, 1);

                        var newLocalFrontTh = localFrontTh + localCompensateTh * Math.Sign(speed);
                        var newLocalRearTh = localRearTh - localCompensateTh * Math.Sign(speed);
                        // VisPointing(Color.Red, localPFront, newLocalFrontTh, 2);
                        // VisPointing(Color.Red, localPRear, newLocalRearTh, 2);

                        var localThDiff = CommonMath.ThDiff(newLocalFrontTh, newLocalRearTh);
                        axisDiffFlag = Math.Abs(localThDiff) > ThConsistentThreshold;

                        if (axisDiffFlag)
                        {
                            var localNormFront = CommonMath.Transform2D(localPFront, newLocalFrontTh + 90, Vector2.UnitX);
                            var localNormRear = CommonMath.Transform2D(localPRear, newLocalRearTh + 90, Vector2.UnitX);
                            var (localIntersect, localCenter) =
                                CommonMath.TwoLinesIntersection(localPFront, localNormFront, localPRear, localNormRear);
                            if (!localIntersect)
                                return FailMotionDecomposition("SendMotion",
                                    $"compensated axes do not intersect frontTh={newLocalFrontTh:F1} rearTh={newLocalRearTh:F1}",
                                    deltaTime);
                            myRotCenter = localCenter;
                            sign = myRotCenter.Y > 1 ? 1 : -1;

                            // SendMotionVisualizer?.DrawLine(Color.Red, myRotCenter, localPFront, width: 2);
                            // SendMotionVisualizer?.DrawLine(Color.Red, myRotCenter, localPRear, width: 2);
                        }
                        else
                        {
                            currentFrontTh = newLocalFrontTh;
                            currentRearTh = newLocalRearTh;
                            thDiff = CommonMath.ThDiff(currentFrontTh, currentRearTh);
                        }
                    }
                }

                _rotCenter = myRotCenter;
                Hedingben.ToastText($"fTh:{frontTh:0.0}, rTh:{rearTh:0.0}, DA:{_originBiasTh:0.00}, " +
                                  $"rc:({_rotCenter.X:0.0},{_rotCenter.Y:0.0}), _rb:{_rearBase}, _fb:{_frontBase}", "rotCenter");
                var r0 = _rotCenter.Length();

                // 计算所有轮子的角度
                bool anyWheelInvalid = false;
                for (var i = 0; i < _steerWheels.Count; i++)
                {
                    var sw = _steerWheels[i];
                    float th;

                    if (axisDiffFlag)
                    {
                        var sTh = (float)(Math.Atan2(sw.Position.Y - _rotCenter.Y, sw.Position.X - _rotCenter.X) /
                            Math.PI * 180) + sign * 90;
                        th = CommonMath.ThDiff(sTh, sw.ZeroDirection);
                        var r1 = Vector2.Distance(sw.Position, _rotCenter);
                        _tmpSpeeds[i] = r1 / (r0 + 0.00001f);
                    }
                    else
                    {
                        var axisTh = currentRearTh + thDiff * Interpolate(_rearBase, _wheelBases[i], _frontBase);
                        _tmpSpeeds[i] = 1;
                        th = CommonMath.ThDiff(axisTh, sw.ZeroDirection);
                    }

                    if (!TryResolveWheelAngle(i, th, "SendMotion", out var resolvedTh, out var resolvedDir,
                            out var resolveReason))
                    {
                        anyWheelInvalid = true;
                        needAdjust = true;
                        lastInvalidReason = resolveReason;
                        break;
                    }

                    _wheelDirs[i] = resolvedDir;
                    tmpAngles.Add(resolvedTh);
                }

                if (needAdjust && anyWheelInvalid)
                {
                    return FailMotionDecomposition("SendMotion", lastInvalidReason, deltaTime);
                }

                finalAngles = tmpAngles;
                break;
            }

            if (finalAngles.Count == _steerWheels.Count)
            {
                for (var i = 0; i < _steerWheels.Count; i++)
                {
                    SendTh(i, finalAngles[i]);
                }
            }
            else
            {
                return FailMotionDecomposition("SendMotion",
                    $"unable to find valid wheel angles frontTh={frontTh:F1} rearTh={rearTh:F1}",
                    deltaTime);
            }

            // // todo: find better way to limit max speed
            // var totalTurn = Math.Abs(frontTh) + Math.Abs(rearTh);
            // var turnThresholdSpeed = CalculateTurningSpeedDecayFac(totalTurn) * MaxSpeed;
            for (int i = 0; i < _steerWheels.Count; i++)
            {
                _debugSpeeds[i] = _tmpSpeeds[i] * speed * _wheelDirs[i];
            }
            var speedBeforeAlignGate = speed;

            if (!GoingWheelAligned)
            {
                Hedingben.ToastText("Wheel Not Aligned", "MultiWheelChassis-SendMotion-notAligned");
                speed = 0;
                var aligned = _steerWheels.Select((sw, i) => (sw, i)).All(ww =>
                    Math.Abs(CommonMath.ThDiff(ww.sw.ReadAngle(), _sendAngle[ww.i])) < 1);
                GoingWheelAligned = aligned;
            }

            for (var i = 0; i < _steerWheels.Count; i++)
            {
                AccumulateSpeed(i, _tmpSpeeds[i] * speed * _wheelDirs[i], axisDiffFlag, _rotCenter, deltaTime);
            }

            if ((DateTime.Now - _sendMotionDetailLastLog).TotalMilliseconds >= 200)
            {
                _sendMotionDetailLastLog = DateTime.Now;
                var sb = new StringBuilder();
                sb.Append($"SendMotionDetail speed:{speed:F3} speedBeforeGate:{speedBeforeAlignGate:F3} fTh:{frontTh:F1} rTh:{rearTh:F1} " +
                          $"curF:{currentFrontTh:F1} curR:{currentRearTh:F1} " +
                          $"originBias:({_originBiasX:F1},{_originBiasY:F1},{_originBiasTh:F1}) " +
                          $"in(speed:{compInputSpeed:F3},f:{compInputFrontTh:F1},r:{compInputRearTh:F1}) " +
                          $"comp({localCompensateX:F1},{localCompensateY:F1},{localCompensateTh:F2}) " +
                          $"motionTh:{compMotionTh:F1} along:{compAlong:F1} side:{compSide:F1} dSpeed:{compDSpeed:F3} dTh:{compDTh:F1} " +
                          $"axisDiff:{axisDiffFlag} rotCenter:({_rotCenter.X:F1},{_rotCenter.Y:F1}) goingAligned:{GoingWheelAligned}");
                for (var i = 0; i < _steerWheels.Count; i++)
                {
                    var sw = _steerWheels[i];
                    var readTh = sw.ReadAngle();
                    var targetTh = _sendAngle[i];
                    var errTh = CommonMath.ThDiff(readTh, targetTh);
                    sb.Append($" | w{i} tgt:{targetTh:F1} read:{readTh:F1} err:{errTh:F1} " +
                              $"zero:{sw.ZeroDirection:F1} rel:{sw.GetAngleRelativeToChassis():F1} " +
                              $"dir:{_wheelDirs[i]} tmp:{_tmpSpeeds[i]:F2} targetV:{_targetSpeeds[i]:F3} sendV:{_sendSpeeds[i]:F3} " +
                              $"lim:[{sw.AngleLowerLimit:F1},{sw.AngleUpperLimit:F1}]");
                    if (sw is DiffSteerWheel dsw)
                    {
                        sb.Append($" diffSend(L:{dsw.GetLeftSendSpeed():F3},R:{dsw.GetRightSendSpeed():F3})");
                    }
                    else
                    {
                        sb.Append($" wheelSend:{sw.GetSendSpeed():F3}");
                    }
                }
                DLog.Log(sb.ToString(), "MultiWheelMotionDetail");
            }

            LastMoveTime = DateTime.Now;
            LastMotionDecomposeFailureReason = "";
            return true;
        }

        /// <summary>
        /// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。
        /// 只下发舵角，不下发驱动速度。
        /// </summary>
        public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f)
        {
            if (!Valid)
                return FailMotionDecomposition(
                    "PrepareRotateWheels",
                    "invalid chassis",
                    null);

            if (float.IsNaN(alignmentToleranceDegrees) ||
                float.IsInfinity(alignmentToleranceDegrees) ||
                alignmentToleranceDegrees < 0.0f)
                throw new ArgumentOutOfRangeException(
                    nameof(alignmentToleranceDegrees),
                    "自转舵轮到位容差必须是非负有限值。");

            if (_steerWheels.Count == 0)
                return FailMotionDecomposition(
                    "PrepareRotateWheels",
                    "no steer wheels",
                    null);

            // 模式切换期间必须保持驱动轮停止。
            PredefinedDriveStop();

            var targetAngles = new float[_steerWheels.Count];
            var directions = new int[_steerWheels.Count];

            // 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。
            for (var i = 0; i < _steerWheels.Count; i++)
            {
                var wheel = _steerWheels[i];
                var px = (double)wheel.Position.X;
                var py = (double)wheel.Position.Y;

                // 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。
                var tangentDegrees =
                    (float)(Math.Atan2(px, -py) /
                            Math.PI * 180.0);
                tangentDegrees = CommonMath.ThDiff(
                    tangentDegrees,
                    wheel.ZeroDirection);

                if (!TryResolveWheelAngle(
                        i,
                        tangentDegrees,
                        "PrepareRotateWheels",
                        out targetAngles[i],
                        out directions[i],
                        out var reason))
                    return FailMotionDecomposition(
                        "PrepareRotateWheels",
                        reason,
                        null);
            }

            for (var i = 0; i < _steerWheels.Count; i++)
            {
                _wheelDirs[i] = directions[i];
                SendTh(i, targetAngles[i]);
            }

            var allAligned = true;
            for (var i = 0; i < _steerWheels.Count; i++)
            {
                var actualAngle = _steerWheels[i].ReadAngle();
                var angleError = targetAngles[i] - actualAngle;

                // 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。
                if (float.IsNaN(actualAngle) ||
                    float.IsInfinity(actualAngle) ||
                    Math.Abs(angleError) > alignmentToleranceDegrees)
                    allAligned = false;
            }

            LastRotateAligned = allAligned;
            LastMotionDecomposeFailureReason = "";
            return true;
        }

        /// <summary>
        /// 将PrepareRotateWheels已经确认到位的舵角和轮速方向，
        /// 原样交接给SendXYThSpeed，作为一段XYTh运动的初始状态。
        /// 该方法不会调用ResetMotionState，因此不会重新选择等价舵角。
        /// </summary>
        public bool AdoptPreparedRotateWheelsForXYTh(
            float alignmentToleranceDegrees = 2.0f)
        {
            if (!Valid)
                return FailMotionDecomposition(
                    "AdoptPreparedRotateWheelsForXYTh",
                    "invalid chassis",
                    null);

            if (float.IsNaN(alignmentToleranceDegrees) ||
                float.IsInfinity(alignmentToleranceDegrees) ||
                alignmentToleranceDegrees < 0.0f)
                throw new ArgumentOutOfRangeException(
                    nameof(alignmentToleranceDegrees),
                    "自转舵轮交接容差必须是非负有限值。");

            if (!LastRotateAligned)
                return FailMotionDecomposition(
                    "AdoptPreparedRotateWheelsForXYTh",
                    "rotate wheels have not been prepared and aligned",
                    null);

            for (var i = 0; i < _steerWheels.Count; i++)
            {
                var actualAngle =
                    _steerWheels[i].ReadAngle();

                if (float.IsNaN(actualAngle) ||
                    float.IsInfinity(actualAngle))
                {
                    LastRotateAligned = false;
                    return FailMotionDecomposition(
                        "AdoptPreparedRotateWheelsForXYTh",
                        $"wheel {i} angle feedback is invalid: {actualAngle}",
                        null);
                }

                // 比较受机械限位约束的实际舵角，不使用圆周最短角。
                var angleError =
                    _sendAngle[i] - actualAngle;
                if (Math.Abs(angleError) >
                    alignmentToleranceDegrees)
                {
                    LastRotateAligned = false;
                    return FailMotionDecomposition(
                        "AdoptPreparedRotateWheelsForXYTh",
                        $"wheel {i} is no longer aligned: " +
                        $"target={_sendAngle[i]:F1}, actual={actualAngle:F1}, " +
                        $"error={angleError:F1}",
                        null);
                }
            }

            // 直接继承PrepareRotateWheels写入的_wheelDirs和_sendAngle。
            // 下一次SendXYThSpeed调用看到XYThActive=true时不会重置这些状态。
            XYThActive = true;
            _xyThWheelsAligned = true;
            GoingActive = false;
            RotatingActive = false;
            LastMoveTime = DateTime.Now;
            LastMotionDecomposeFailureReason = "";
            return true;
        }

        /// <summary>
        /// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。
        /// </summary>
        /// <param name="rotSpeed">绕车队中心角速度(deg/s，逆时针为正)。</param>
        /// <param name="localCompensateX">车体系纵向(前+)修正速度(mm/s)，多车联动维持队形用。</param>
        /// <param name="localCompensateY">车体系横向(左+)修正速度(mm/s)。</param>
        /// <param name="localCompensateTh">绕本车几何中心附加角速度(deg/s)，修正朝向偏差。</param>
        public bool SendRotateMotion(float rotSpeed, TimeSpan? deltaTime = null,
            float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f)
        {
            if (!Valid) return FailMotionDecomposition("SendRotateMotion", "invalid chassis", deltaTime);
            LastMotionDecomposeFailureReason = "";
            if (Math.Abs(rotSpeed) < 1e-6f &&
                Math.Abs(localCompensateX) < 1e-6f &&
                Math.Abs(localCompensateY) < 1e-6f &&
                Math.Abs(localCompensateTh) < 1e-6f)
            {
                LastRotateAligned = true;
                RampStop(deltaTime);
                return true;
            }

            var ths = new float[_steerWheels.Count];
            var dirs = new int[_steerWheels.Count];
            // 每轮合速度大小(m/s)，含"绕队心旋转 + 车体平移纠偏 + 绕本车中心微转纠偏"三项矢量和。
            var speedMags = new float[_steerWheels.Count];
            var allWheelAligned = true;

            // 把车体系纠偏旋量换算到 sw.Position 所在的偏置帧 F(原点=车队中心, 朝向随 _originBiasTh)。
            // body->F 旋转为 R(_originBiasTh)，与 SetOriginBias 里 Transform2D 的约定一致。
            var radBias = _originBiasTh / 180.0 * Math.PI;
            var cosB = Math.Cos(radBias);
            var sinB = Math.Sin(radBias);
            var compVxF = localCompensateX * cosB - localCompensateY * sinB; // mm/s，F帧
            var compVyF = localCompensateX * sinB + localCompensateY * cosB;
            var rotRad = rotSpeed / 180.0 * Math.PI;            // 绕队心(F原点)角速度 rad/s
            var compRad = localCompensateTh / 180.0 * Math.PI;  // 绕本车几何中心附加角速度 rad/s
            var biasX = (double)_originBiasX;                   // 本车几何中心在 F 中的位置
            var biasY = (double)_originBiasY;

            for (var i = 0; i < _steerWheels.Count; i++)
            {
                var sw = _steerWheels[i];
                var px = (double)sw.Position.X;
                var py = (double)sw.Position.Y;
                // 合成轮速矢量(mm/s, F帧)：v = ω_rot ẑ×p + [R(bias)·v_comp + ω_comp ẑ×(p - 本车中心)]
                // 旋转切向项与纠偏项分开算，便于把纠偏钳到旋转切向的一定比例。
                var vxRot = -rotRad * py;
                var vyRot = rotRad * px;
                var vxComp = compVxF - compRad * (py - biasY);
                var vyComp = compVyF + compRad * (px - biasX);
                // 纠偏钳位：|纠偏| ≤ RotateCompTangentFrac × |旋转切向|，限制合矢量相对纯切向的最大偏角
                // (frac=0.5 → 偏角≤26.6°)，避免减速末段纠偏盖过旋转切向导致舵轮 180° 乱翻。
                if (RotateCompTangentFrac >= 0f)
                {
                    var rotMag = Math.Sqrt(vxRot * vxRot + vyRot * vyRot);
                    var compMag = Math.Sqrt(vxComp * vxComp + vyComp * vyComp);
                    var compLimit = RotateCompTangentFrac * rotMag;
                    if (compMag > compLimit && compMag > 1e-6)
                    {
                        var s = compLimit / compMag;
                        vxComp *= s;
                        vyComp *= s;
                    }
                }
                var vx = vxRot + vxComp;
                var vy = vyRot + vyComp;
                var vmag = Math.Sqrt(vx * vx + vy * vy);
                speedMags[i] = (float)(vmag / 1000.0);

                // 轮子应指向的运动方向(F帧)。纠偏后合速度≈0 时退化为纯旋转切向，避免 atan2 抖动。
                var angleF = vmag < 1e-6
                    ? Math.Atan2(px, -py) / Math.PI * 180
                    : Math.Atan2(vy, vx) / Math.PI * 180;
                var tangent = CommonMath.ThDiff((float)angleF, sw.ZeroDirection);

                if (!TryResolveWheelAngle(i, tangent, "SendRotateMotion", out ths[i], out dirs[i],
                        out var resolveReason))
                    return FailMotionDecomposition("SendRotateMotion", resolveReason, deltaTime);
                _wheelDirs[i] = dirs[i];
            }

            for (var i = 0; i < _steerWheels.Count; ++i)
                SendTh(i, ths[i]);
            var slowFac = 1f;
            var maxDth = 0f;
            for (var i = 0; i < _steerWheels.Count; ++i)
            {
                var actualAngle = _steerWheels[i].ReadAngle();
                var dth = Math.Abs(ths[i] - actualAngle);
                maxDth = Math.Max(maxDth, dth);
                slowFac = Math.Min(
                    slowFac,
                    CommonMath.gaussmf(
                        dth,
                        Math.Max(SteeringAlignmentSigmaDegrees, 0.1f),
                        0));
                if (float.IsNaN(actualAngle) ||
                    float.IsInfinity(actualAngle) ||
                    dth > 2.0f)
                    allWheelAligned = false;
            }
            LastRotateAligned = allWheelAligned; // 供上层做积分抗饱和

            // 对齐门控：未对齐则整体停车；对齐后用 slowFac 平滑提速。旋转与纠偏被同等缩放，保持几何一致。
            var gateFac = allWheelAligned ? slowFac : 0f;
            for (var i = 0; i < _steerWheels.Count; ++i)
                AccumulateSpeed(i, speedMags[i] * gateFac * dirs[i], true, Vector2.Zero, deltaTime);

            // === 原地旋转诊断（节流 ~300ms）===
            // 关注 allWheelAligned（false 说明舵轮追不上目标角）、dirs/半径，以及叠加的纠偏旋量 comp(vx,vy,om)。
            if ((DateTime.Now - _rotDbgLast).TotalMilliseconds >= 300)
            {
                _rotDbgLast = DateTime.Now;
                var sb = new StringBuilder();
                sb.Append($"SendRotateMotion in:{rotSpeed:F1} aligned:{allWheelAligned} maxDth:{maxDth:F1} slowFac:{slowFac:F2} gate:{gateFac:F2} biasTh:{_originBiasTh:F1} comp(vx:{localCompensateX:F1} vy:{localCompensateY:F1} om:{localCompensateTh:F1})");
                for (var i = 0; i < _steerWheels.Count; ++i)
                {
                    var sw = _steerWheels[i];
                    sb.Append($" | w{i} tgt:{ths[i]:F1} read:{sw.ReadAngle():F1} dth:{Math.Abs(CommonMath.ThDiff(ths[i], sw.ReadAngle())):F1} dir:{dirs[i]} R:{sw.Position.Length():F0} v:{speedMags[i] * gateFac * dirs[i]:F3} send:{sw.GetSendSpeed():F2}");
                }
                DLog.Log(sb.ToString(), "RotateDbg");
            }

            LastMoveTime = DateTime.Now;
            LastMotionDecomposeFailureReason = "";
            return true;
        }

        private DateTime _rotDbgLast = DateTime.MinValue;

        public override float CalculateTurningSpeedDecayFac(float turn)
        {
            return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
        }

        [Obsolete]
        public List<SteerWheel> GetSteerWheels()
        {
            return _steerWheels.ToList();
        }

        private void SendTh(int i, float targetTh)
        {
            _steerWheels[i].WriteAngle(targetTh);
            _sendAngle[i] = targetTh;
        }

        private (int direction, float rangeFront, float rangeRear) DetermineWheelDirection(SteerWheel wheel)
        {
            var lower = CommonMath.RoundTh(wheel.AngleLowerLimit + wheel.ZeroDirection);
            var upper = CommonMath.RoundTh(wheel.AngleUpperLimit + wheel.ZeroDirection);
            while (upper < lower) upper += 360;

            var frontDir = 0;
            var rearDir = 180;

            // 计算前向和后向的可用范围
            var rangeFront = Math.Min(upper - frontDir, frontDir - lower);
            var rangeRear = Math.Min(upper - rearDir, rearDir - lower);

            // 选择范围更大的方向
            if (rangeFront >= rangeRear && rangeFront > 0)
            {
                return (1, rangeFront, rangeRear);
            }
            else if (rangeRear > rangeFront && rangeRear > 0)
            {
                return (-1, rangeFront, rangeRear);
            }

            // 如果两个方向都不可行
            return (0, rangeFront, rangeRear);
        }

        private void AccumulateSpeed(int i, float v, bool axisDiff, Vector2 rotCenter, TimeSpan? deltaTime = null)
        {
            _targetSpeeds[i] = v;
            var speedSign = Math.Sign(_targetSpeeds[i] - _sendSpeeds[i]);
            var acc = Math.Abs(_targetSpeeds[i]) > Math.Abs(_sendSpeeds[i]) ? AccPerSecond : DeAccPerSecond;
            _sendSpeeds[i] += speedSign * Math.Min(Math.Abs(_targetSpeeds[i] - _sendSpeeds[i]),
                acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds);
            if (_steerWheels[i] is DiffSteerWheel dsw)
            {
                if (axisDiff)
                {
                    var wheelRadius = Vector2.Distance(rotCenter, dsw.Position);
                    var wheelDir = _sendAngle[i];

                    var left = CommonMath.Transform2D(dsw.Position, wheelDir + 90, new Vector2(dsw.WheelDistance, 0));
                    var right = CommonMath.Transform2D(dsw.Position, wheelDir - 90, new Vector2(dsw.WheelDistance, 0));
                    dsw.WriteLeftSpeed(_sendSpeeds[i] / wheelRadius * Vector2.Distance(rotCenter, left));
                    dsw.WriteRightSpeed(_sendSpeeds[i] / wheelRadius * Vector2.Distance(rotCenter, right));
                }
                else
                {
                    dsw.WriteLeftSpeed(_sendSpeeds[i]);
                    dsw.WriteRightSpeed(_sendSpeeds[i]);
                }
            }
            else
            {
                _steerWheels[i].WriteSpeed(_sendSpeeds[i]);
            }

            if (Debug)
                Console.WriteLine($"Ackermann wheel{i}: target:{_targetSpeeds[i]:0.00},send:{_sendSpeeds[i]:0.0}");
        }

        private void CalculateAxes()
        {
            var axes = _steerWheels
                .Select(sw => (sw, Vector2.Dot(sw.Position, new Vector2(1, 0))))
                .OrderByDescending(ax => ax.Item2).ToList();
            _wheelBases = axes.Select(ax => ax.Item2).ToList();
            _steerWheels = axes.Select(ax => ax.sw).ToList();
            GeometricControlPoints = new List<GeometricControlPoint>()
            {
                new (new Vector2(ControlPointRadius, 0)),
                new (new Vector2(-ControlPointRadius, 0))
            };
            _frontBase = _wheelBases.First();
            _rearBase = _wheelBases.Last();
        }

        private List<SteerWheel> _steerWheels = new();
        private float _frontBase, _rearBase;
        private List<float> _wheelBases;
        private List<float> _sendSpeeds;
        private List<float> _targetSpeeds;
        private List<float> _sendAngle;
        private List<float> _debugSpeeds;
        private DateTime _sendMotionDetailLastLog = DateTime.MinValue;
        private DateTime _geometricComputeLastLog = DateTime.MinValue;

        private List<float> _tmpSpeeds;
        // add TimeStamp, prevent the wheel from swaying.
        // for example, target angle is 90, current wheel is around 0. if no TimeStamp, 
        // wheel will swing between 90 and -90.
        private List<int> _wheelDirs;

        // call this before a new motion sequence happens
        private void ResetMotionState()
        {
            LastMoveTime = DateTime.Now;
            GoingWheelAligned = false;
            _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList();
        }

        public void AddTestFunction()
        {

        }
        /// <summary>
        /// 得到相对舵轮在车体坐标系下的分解速度
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="vx"></param>
        /// <param name="vy"></param>
        /// <param name="vth">单位为°/s</param>
        /// <returns></returns>
        private Vector2 VectorVelocity(Vector2 pos, float vx, float vy, float vth, int i)
        {
            var vRotX = -vth / 180 * (float)Math.PI * pos.Y / 1000;
            var vRotY = vth / 180 * (float)Math.PI * pos.X / 1000;
            return new Vector2(vx + vRotX, vy + vRotY);
        }
        /// <summary>
        /// 获得车轮应该打的角度和速度
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="vx"></param>
        /// <param name="vy"></param>
        /// <param name="vth">单位为°/s</param>
        /// <returns></returns>
        private (float angle, float speed) AngleAndSpeed(Vector2 pos, float vx, float vy, float vth, int i)
        {
            var v = VectorVelocity(pos, vx, vy, vth, i);
            return ((float)(Math.Atan2(v.Y, v.X) / Math.PI * 180), v.Length());
        }

        /// <summary>
        /// 原地旋转时舵角误差对应的速度衰减宽度，单位为度。
        /// </summary>
        public float SteeringAlignmentSigmaDegrees { get; set; } = 8f;
        /// <summary>
        /// 单轮速度低于此值时认为其运动方向无意义，单位为m/s。
        /// </summary>
        public float WheelDirectionDeadbandMetersPerSecond { get; set; } = 0.005f;
        private bool XYThActive = false;
        private bool _xyThWheelsAligned = false;
        private DateTime _xyThDiagnosticsLastTime = DateTime.MinValue;

        /// <summary>
        /// 下发车体二维速度，并根据舵轮机械角度误差进行高斯降速。
        /// 一段运动开始时必须先等待全部舵轮到位；运动过程中舵角误差越大，
        /// 四轮驱动速度的统一缩放比例越小，适合作为默认安全接口。
        /// vx、vy单位为m/s，vth单位为°/s。
        /// </summary>
        public bool SendXYThSpeed(
            float vx,
            float vy,
            float vth,
            TimeSpan? deltaTime = null,
            bool enableDifferentialSteerFeedforward = false)
        {
            const string operationName = "SendXYThSpeed";

            if (!Valid)
                return FailMotionDecomposition(
                    operationName,
                    "invalid chassis",
                    deltaTime);

            if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f)
            {
                RampStop(deltaTime);
                XYThActive = false;
                _xyThWheelsAligned = false;
                GoingActive = false;
                RotatingActive = false;
                LastMotionDecomposeFailureReason = "";
                return true;
            }

            if (!XYThActive)
            {
                ResetMotionState();
                _xyThWheelsAligned = false;
            }
            XYThActive = true;
            GoingActive = false;
            RotatingActive = false;

            float[] sendSpeed = new float[_steerWheels.Count];
            var allWheelsAligned = true;
            var maximumAngleError = 0f;
            var alignmentSpeedScale = 1f;
            const float initialAlignmentToleranceDegrees = 2f;
            var writeDiagnostics =
                Debug &&
                (DateTime.Now - _xyThDiagnosticsLastTime)
                    .TotalMilliseconds >= 250.0;

            for (var i = 0; i < _steerWheels.Count; i++)
            {
                var sw = _steerWheels[i];
                var (angle, speed) = AngleAndSpeed(sw.Position, vx, vy, vth, i);
                // 单轮合成速度接近零时，运动方向没有物理意义。
                // 此时不重新计算和下发舵角，保持上一目标舵角，轮速降为零。
                var directionDeadband = Math.Max(
                    WheelDirectionDeadbandMetersPerSecond,
                    0f);

                if (speed < directionDeadband)
                {
                    sendSpeed[i] = 0f;

                    if (writeDiagnostics)
                    {
                        Hedingben.ToastText(
                            $"hold-angle speed:{speed:F4} deadband:{directionDeadband:F4}",
                            $"{operationName}-{i}");
                    }

                    continue;
                }
                var actualTh = sw.ReadAngle();

                if (float.IsNaN(actualTh) ||
                    float.IsInfinity(actualTh))
                {
                    return FailMotionDecomposition(
                        operationName,
                        $"wheel {i} angle feedback is invalid: {actualTh}",
                        deltaTime);
                }

                if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), operationName,
                        out var useAngle, out var dir, out var resolveReason))
                    return FailMotionDecomposition(operationName, resolveReason, deltaTime);
                speed *= dir;
                _wheelDirs[i] = dir;

                sendSpeed[i] = speed;
                SendTh(i, useAngle);

                // 这里比较受机械限位约束的实际舵角，不使用圆周最短角。
                var angleError =
                    Math.Abs(_sendAngle[i] - actualTh);
                maximumAngleError =
                    Math.Max(maximumAngleError, angleError);

                alignmentSpeedScale = Math.Min(
                    alignmentSpeedScale,
                    CommonMath.gaussmf(
                        angleError,
                        Math.Max(
                            SteeringAlignmentSigmaDegrees,
                            0.1f),
                        0));
                if (angleError >
                    initialAlignmentToleranceDegrees)
                {
                    allWheelsAligned = false;
                }

                if (writeDiagnostics)
                {
                    Hedingben.ToastText(
                        $"ready:{_xyThWheelsAligned} " +
                        $"err:{angleError:F1} scale:{alignmentSpeedScale:F2} " +
                        $"s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}",
                        $"{operationName}-{i}");
                }
            }

            // 一段XYTh运动刚开始时必须等待全部舵轮到位。
            if (!_xyThWheelsAligned &&
                allWheelsAligned)
            {
                _xyThWheelsAligned = true;
            }

            var driveScale = _xyThWheelsAligned
                ? alignmentSpeedScale
                : 0f;
            #region 新增虚拟旋转中心
            // vth传入单位为deg/s，这里换算成rad/s。
            var omegaRadiansPerSecond =
                vth * (float)Math.PI / 180f;

            // 只有调用者主动开启并且存在旋转运动时，
            // 才启用差速舵轮左右轮的几何速度前馈。
            var useDifferentialSteerFeedforward =
                enableDifferentialSteerFeedforward &&
                Math.Abs(omegaRadiansPerSecond) > 1e-4f;

            var rotationCenter = Vector2.Zero;

            if (useDifferentialSteerFeedforward)
            {
                // 根据车体速度场：
                // vx(point) = vx - omega * y
                // vy(point) = vy + omega * x
                // 计算车体坐标系中的瞬时旋转中心。
                //
                // vx、vy单位为m/s，计算结果原本是m；
                // 舵轮Position使用mm，因此乘以1000。
                rotationCenter = new Vector2(
                    -vy / omegaRadiansPerSecond * 1000f,
                     vx / omegaRadiansPerSecond * 1000f);
            }
            #endregion


            for (var i = 0; i < _steerWheels.Count; i++)
                AccumulateSpeed(
                    i,
                    driveScale *
                    sendSpeed[i],
                    useDifferentialSteerFeedforward,
                    rotationCenter,
                    deltaTime);

            if (writeDiagnostics)
            {
                _xyThDiagnosticsLastTime = DateTime.Now;
                Hedingben.ToastText(
                    $"ready:{_xyThWheelsAligned} " +
                    $"maxErr:{maximumAngleError:F1} scale:{driveScale:F2} " +
                    $"cmd:({vx:F3},{vy:F3},{vth:F1})",
                    $"{operationName}-alignment");
            }
            //todo 计算rotCenter填入
            LastMoveTime = DateTime.Now;
            LastMotionDecomposeFailureReason = "";
            return true;
        }

        public override CarSpeed GetCarSpeed(bool isActual = false)
        {
            List<float> vx = new List<float>();
            List<float> vy = new List<float>();
            List<float> vth = new List<float>();
            for (var i = 0; i < _steerWheels.Count; ++i)
            {
                var sw1 = _steerWheels[i];
                Vector2 a = sw1.Position / 1000;
                //var tha = _sendAngle[i] / 180 * (float)Math.PI;
                var tha = isActual ? sw1.ReadAngle() / 180 * (float)Math.PI : _sendAngle[i] / 180 * (float)Math.PI;
                //var speeda = _sendSpeeds[i];
                //var speeda = isActual ? sw1.ReadSpeed() : _sendSpeeds[i];
                var speeda = isActual ? sw1.ReadSpeed() : _debugSpeeds[i];
                Vector2 va = new Vector2(speeda * (float)Math.Cos(tha), speeda * (float)Math.Sin(tha));
                // painter.DrawLine(Color.Green, sw1.Position,
                //     LessMath.Transform2D(sw1.Position, sw1.ReadAngle(), new Vector2(200, 0)), endArrow: true);
                for (var j = i + 1; j < _steerWheels.Count; ++j)
                {
                    var sw2 = _steerWheels[j];
                    Vector2 b = sw2.Position / 1000;
                    //var thb = _sendAngle[j] / 180 * (float)Math.PI;
                    var thb = isActual ? sw2.ReadAngle() / 180 * (float)Math.PI : _sendAngle[j] / 180 * (float)Math.PI;
                    //var speedb = _sendSpeeds[j];
                    //var speedb = isActual ? sw2.ReadSpeed() : _sendSpeeds[j];
                    var speedb = isActual ? sw2.ReadSpeed() : _debugSpeeds[j];
                    Vector2 vb = new Vector2(speedb * (float)Math.Cos(thb), speedb * (float)Math.Sin(thb));
                    var (tempvx, tempvy, tempvth) = CenterVelocityFromPoints(a, va, b, vb);
                    vx.Add(tempvx);
                    vy.Add(tempvy);
                    vth.Add(tempvth);
                }
            }
            return new CarSpeed()
            { Vx = vx.Average(), Vy = vy.Average(), Vw = (float)(vth.Average() / Math.PI * 180f) };
        }

        private static (float, float, float) CenterVelocityFromPoints(Vector2 a, Vector2 va, Vector2 b, Vector2 vb)
        {
            float vth_x = 0, vth_y = 0, vth = 0, vx = 0, vy = 0;
            var eps = 0.0000001;
            if (Math.Abs(a.Y - b.Y) > eps)
            {
                vth_x = (va.X - vb.X) / (b.Y - a.Y);
            }

            if (Math.Abs(a.X - b.X) > eps)
            {
                vth_y = (va.Y - vb.Y) / (a.X - b.X);
            }
            vth = vth_x == 0 ? vth_y : vth_x;
            vx = va.X + vth * a.Y;
            vy = va.Y - vth * a.X;
            return (vx, vy, vth);
        }
    }
}