// 将统一命令转换为原 Chassis API 调用
using System;
using CommonUsage.Chassis;

namespace MyParking.Shared
{
    /// <summary>
    /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
    /// 车体坐标系固定为X向前、Y向左、逆时针为正。
    /// </summary>
    public sealed class MultiWheelChassisAdapter
    {
        #region 辅助内容
        private const float BiasTolerance = 0.001f;
        private const double MotionDeadband = 1e-6;
        private readonly MultiWheelChassis _chassis;
        private double _activeMotionDirectionRadians;
        /// <summary>
        /// 当前适配器对应的车辆编号。
        /// </summary>
        public int VehicleId { get; }

        /// <summary>
        /// Maximum distance from the body origin to a wheel center, in metres.
        /// </summary>
        public double MaximumWheelRadiusMeters { get; }

        /// <summary>
        /// Maximum longitudinal wheel offset from the body origin, in metres.
        /// For a symmetric four-wheel-steering chassis this is half the wheelbase.
        /// </summary>
        public double HalfWheelBaseMeters { get; }

        /// <summary>
        /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
        /// 对称四舵轮底盘中，它通常等于物理轮距的一半。
        /// </summary>
        public double HalfTrackWidthMeters { get; }

        /// <summary>
        /// 获取旧版SendMotion使用的对称前后GCP半径，单位为m。
        /// </summary>
        public double ControlPointRadiusMeters =>
            _chassis.ControlPointRadius / 1000.0;

        /// <summary>
        /// 获取当前已经准备并激活的滚动运动系X轴在真实车体系中的方向，单位为rad。
        /// </summary>
        public double ActiveMotionDirectionRadians =>
            _activeMotionDirectionRadians;

        /// <summary>
        /// Width of the steering-alignment speed gate, in degrees.
        /// </summary>
        public double SteeringAlignmentSigmaDegrees
        {
            get => _chassis.SteeringAlignmentSigmaDegrees;
            set
            {
                NumericGuard.EnsureFinitePositive(
                    value,
                    nameof(value));
                EnsureRepresentableAsSingle(
                    value,
                    nameof(value));

                _chassis.SteeringAlignmentSigmaDegrees =
                    (float)value;
            }
        }

        /// <summary>
        /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
        /// </summary>
        private void EnsureBodyFrameIsActive()
        {
            EnsureMotionFrameIsActive(0.0);
        }

        /// <summary>
        /// 检查旧底盘当前是否处于指定的运动坐标系。
        /// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
        /// </summary>
        private void EnsureMotionFrameIsActive(
            double motionDirectionRadians)
        {
            NumericGuard.EnsureFinite(
                motionDirectionRadians,
                nameof(motionDirectionRadians));

            var expectedBiasDegrees =
                ConvertRadiansToSingleDegrees(
                    -AngleMath.NormalizeRadians(
                        motionDirectionRadians),
                    nameof(motionDirectionRadians));
            var bias = _chassis.GetOriginBias();
            var angleErrorDegrees =
                AngleMath.NormalizeDegrees(
                    bias.Z - expectedBiasDegrees);

            if (Math.Abs(bias.X) <= BiasTolerance &&
                Math.Abs(bias.Y) <= BiasTolerance &&
                Math.Abs(angleErrorDegrees) <=
                BiasTolerance)
            {
                return;
            }

            throw new InvalidOperationException(
                "MultiWheelChassis当前运动坐标系与命令不一致。" +
                $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°，" +
                $"期望Th={expectedBiasDegrees}°。");
        }

        /// <summary>
        /// 检查底盘命令是否包含无效数值。
        /// </summary>
        private static void ValidateTwist(Twist2D twist)
        {
            EnsureRepresentableAsSingle(
                twist.VxMetersPerSecond,
                nameof(twist.VxMetersPerSecond));

            EnsureRepresentableAsSingle(
                twist.VyMetersPerSecond,
                nameof(twist.VyMetersPerSecond));

            EnsureRepresentableAsSingle(
                AngleMath.RadiansToDegrees(
                    twist.OmegaRadiansPerSecond),
                nameof(twist.OmegaRadiansPerSecond));
        }
        /// <summary>
        /// 检查数值是否为有限值且可安全转换为float。
        /// </summary>
        private static void EnsureRepresentableAsSingle(
            double value,
            string parameterName)
        {
            NumericGuard.EnsureFinite(value, parameterName);

            if (value > float.MaxValue ||
                value < -float.MaxValue)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "底盘速度命令超过float可表示范围。");
            }
        }

        /// <summary>
        /// 将有限弧度值转换为float可表示的角度值。
        /// </summary>
        private static float ConvertRadiansToSingleDegrees(
            double angleRadians,
            string parameterName)
        {
            var angleDegrees =
                AngleMath.RadiansToDegrees(angleRadians);
            EnsureRepresentableAsSingle(
                angleDegrees,
                parameterName);
            return (float)angleDegrees;
        }

        /// <summary>
        /// 获取最近一次底盘运动分解失败原因。
        /// </summary>
        public string LastFailureReason =>
            _chassis.LastMotionDecomposeFailureReason;

        #endregion

        /// <summary>
        /// 将旧底盘的原点偏置恢复为真实单车车体坐标系。
        /// </summary>
        public void ResetToBodyFrame()
        {
            ActivateMotionFrame(0.0);
        }

        /// <summary>
        /// 激活指定运动方向对应的SendMotion坐标系。
        /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
        /// 调用方必须先停车，并确认舵轮已经按该方向完成预对齐。
        /// </summary>
        public void ActivateMotionFrame(
            double motionDirectionRadians)
        {
            NumericGuard.EnsureFinite(
                motionDirectionRadians,
                nameof(motionDirectionRadians));

            var normalizedDirectionRadians =
                AngleMath.NormalizeRadians(
                    motionDirectionRadians);

            var biasDegrees =
                ConvertRadiansToSingleDegrees(
                    -normalizedDirectionRadians,
                    nameof(motionDirectionRadians));
            var currentBias =
                _chassis.GetOriginBias();

            if (Math.Abs(currentBias.X) <=
                    BiasTolerance &&
                Math.Abs(currentBias.Y) <=
                    BiasTolerance &&
                Math.Abs(
                    AngleMath.NormalizeDegrees(
                        currentBias.Z -
                        biasDegrees)) <=
                    BiasTolerance)
            {
                SetActiveMotionDirection(
                    normalizedDirectionRadians);
                return;
            }

            _chassis.SetOriginBias(
                x: 0.0f,
                y: 0.0f,
                th: biasDegrees);
            SetActiveMotionDirection(
                normalizedDirectionRadians);
        }

        /// <summary>
        /// 缓存当前运动坐标系方向，供控制周期内转换车体速度。
        /// </summary>
        private void SetActiveMotionDirection(
            double motionDirectionRadians)
        {
            _activeMotionDirectionRadians =
                AngleMath.NormalizeRadians(
                    motionDirectionRadians);
        }
        public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
        {
            _chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
            if (vehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(vehicleId),
                    "车辆编号必须大于零。");
            }
            VehicleId = vehicleId;
#pragma warning disable CS0612, CS0618
            var wheels = _chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618

            if (wheels.Count == 0)
            {
                throw new InvalidOperationException(
                    "MultiWheelChassis尚未完成舵轮初始化，" +
                    "不能创建底盘适配器。");
            }
            // 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
            // 保证SendXYThSpeed直接使用真实车体坐标系。
            var maximumWheelRadiusMillimeters = 0.0;
            var maximumLongitudinalOffsetMillimeters = 0.0;
            var maximumLateralOffsetMillimeters = 0.0;
            foreach (var wheel in wheels)
            {
                maximumWheelRadiusMillimeters = Math.Max(
                    maximumWheelRadiusMillimeters,
                    wheel.PhysicalPosition.Length());

                maximumLongitudinalOffsetMillimeters = Math.Max(
                    maximumLongitudinalOffsetMillimeters,
                    Math.Abs(wheel.PhysicalPosition.X));

                maximumLateralOffsetMillimeters = Math.Max(
                    maximumLateralOffsetMillimeters,
                    Math.Abs(wheel.PhysicalPosition.Y));
            }

            MaximumWheelRadiusMeters =
                maximumWheelRadiusMillimeters / 1000.0;
            HalfWheelBaseMeters =
                maximumLongitudinalOffsetMillimeters / 1000.0;
            HalfTrackWidthMeters =
                maximumLateralOffsetMillimeters / 1000.0;

            if (MaximumWheelRadiusMeters <= 0.0 ||
                HalfWheelBaseMeters <= 0.0 ||
                HalfTrackWidthMeters <= 0.0 ||
                ControlPointRadiusMeters <= 0.0)
            {
                throw new InvalidOperationException(
                    "Wheel positions and ControlPointRadius must produce valid chassis dimensions.");
            }

            var initialBias = _chassis.GetOriginBias();
            SetActiveMotionDirection(
                -AngleMath.DegreesToRadians(
                    initialBias.Z));

            // 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。
            _chassis.PreferMinimumSteeringTravel = true;
        }

        /// <summary>
        /// 将车体坐标系刚体速度统一转换为滚动SendMotion、原地自转或停车命令。
        /// 非零平移命令使用调用方在运动段开始前已经准备并激活的β运动坐标系。
        /// </summary>
        public bool SendBodyTwist(
            Twist2D bodyTwist,
            TimeSpan? interval = null)
        {
            ValidateTwist(bodyTwist);

            var linearSpeedMetersPerSecond =
                Math.Sqrt(
                    bodyTwist.VxMetersPerSecond *
                    bodyTwist.VxMetersPerSecond +
                    bodyTwist.VyMetersPerSecond *
                    bodyTwist.VyMetersPerSecond);

            if (linearSpeedMetersPerSecond <= MotionDeadband)
            {
                if (Math.Abs(
                        bodyTwist.OmegaRadiansPerSecond) <=
                    MotionDeadband)
                {
                    StopImmediately();
                    return true;
                }

                return SendPureRotation(
                    bodyTwist.OmegaRadiansPerSecond,
                    interval);
            }

            return SendRollingTwistInActiveMotionFrame(
                bodyTwist,
                linearSpeedMetersPerSecond,
                interval);
        }

        /// <summary>
        /// 将车体刚体速度转换到已准备的运动坐标系，并生成该坐标系中的前后GCP方向。
        /// </summary>
        private bool SendRollingTwistInActiveMotionFrame(
            Twist2D bodyTwist,
            double linearSpeedMetersPerSecond,
            TimeSpan? interval)
        {
            EnsureMotionFrameIsActive(
                _activeMotionDirectionRadians);

            var bodyPoseInMotionFrame =
                new Pose2D(
                    0.0,
                    0.0,
                    -_activeMotionDirectionRadians);
            var motionTwist =
                FrameTransform2D.TransformTwistAtSamePoint(
                    bodyPoseInMotionFrame,
                    bodyTwist);
            var motionVxMetersPerSecond =
                motionTwist.VxMetersPerSecond;
            var motionVyMetersPerSecond =
                motionTwist.VyMetersPerSecond;

            if (Math.Abs(motionVxMetersPerSecond) <=
                MotionDeadband)
            {
                StopImmediately();
                throw new InvalidOperationException(
                    "当前车体速度几乎垂直于已经准备的运动坐标系，" +
                    "无法由方向型前后GCP稳定表示。请停车后按目标主运动方向重新准备并激活β。");
            }

            var travelDirection =
                Math.Sign(
                    motionVxMetersPerSecond);
            var signedCenterSpeedMetersPerSecond =
                travelDirection *
                linearSpeedMetersPerSecond;
            var frontVelocityYMetersPerSecond =
                motionVyMetersPerSecond +
                bodyTwist.OmegaRadiansPerSecond *
                ControlPointRadiusMeters;
            var rearVelocityYMetersPerSecond =
                motionVyMetersPerSecond -
                bodyTwist.OmegaRadiansPerSecond *
                ControlPointRadiusMeters;
            var directedVxMetersPerSecond =
                travelDirection *
                motionVxMetersPerSecond;
            var frontAngleRadians =
                Math.Atan2(
                    travelDirection *
                    frontVelocityYMetersPerSecond,
                    directedVxMetersPerSecond);
            var rearAngleRadians =
                Math.Atan2(
                    travelDirection *
                    rearVelocityYMetersPerSecond,
                    directedVxMetersPerSecond);

            return SendGcpMotionInActiveFrame(
                signedCenterSpeedMetersPerSecond,
                frontAngleRadians,
                rearAngleRadians,
                interval);
        }

        /// <summary>
        /// 将已经完成自转舵轮准备的纯角速度命令交给XYTh底盘解算。
        /// </summary>
        private bool SendPureRotation(
            double omegaRadiansPerSecond,
            TimeSpan? interval)
        {
            EnsureBodyFrameIsActive();

            var success = _chassis.SendXYThSpeed(
                0.0f,
                0.0f,
                ConvertRadiansToSingleDegrees(
                    omegaRadiansPerSecond,
                    nameof(omegaRadiansPerSecond)),
                interval,
                enableDifferentialSteerFeedforward: true);

            if (!success)
            {
                _chassis.PredefinedDriveStop();
            }

            return success;
        }
        /// <summary>
        /// 在当前已激活的运动坐标系中将有符号速度和前后GCP角度发送给旧版SendMotion。
        /// </summary>
        private bool SendGcpMotionInActiveFrame(
            double speedMetersPerSecond,
            double frontAngleRadians,
            double rearAngleRadians,
            TimeSpan? interval = null)
        {
            EnsureRepresentableAsSingle(
                speedMetersPerSecond,
                nameof(speedMetersPerSecond));
            NumericGuard.EnsureFinite(
                frontAngleRadians,
                nameof(frontAngleRadians));
            NumericGuard.EnsureFinite(
                rearAngleRadians,
                nameof(rearAngleRadians));
            EnsureMotionFrameIsActive(
                _activeMotionDirectionRadians);

            if (Math.Abs(frontAngleRadians) >=
                    Math.PI / 2.0 ||
                Math.Abs(rearAngleRadians) >=
                    Math.PI / 2.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(frontAngleRadians),
                    "前后GCP角度必须位于正负90度以内，避免四轮几何解算出现奇异值。");
            }

            var success = _chassis.SendMotion(
                (float)speedMetersPerSecond,
                ConvertRadiansToSingleDegrees(
                    frontAngleRadians,
                    nameof(frontAngleRadians)),
                ConvertRadiansToSingleDegrees(
                    rearAngleRadians,
                    nameof(rearAngleRadians)),
                interval);

            if (!success)
            {
                // 分解失败后立即清除上一条驱动速度，避免车辆继续执行陈旧命令。
                _chassis.PredefinedDriveStop();
            }

            return success;
        }

        /// <summary>
        /// 立即将所有驱动轮速度下发为零。
        /// </summary>
        public void StopImmediately()
        {
            _chassis.PredefinedDriveStop();
        }

        /// <summary>
        /// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。
        /// </summary>
        public void StopXYThDrivePreserveSteeringState()
        {
            _chassis.StopXYThDrivePreserveSteeringState();
        }

        /// <summary>
        /// 停车并将所有舵轮转到指定的车体角度。
        /// 只调整舵轮角度，不产生车辆线速度。
        /// </summary>
        public bool PrepareParallelDirection(
            double directionRadians)
        {
            EnsureBodyFrameIsActive();
            var targetDegrees =
                ConvertRadiansToSingleDegrees(
                    AngleMath.NormalizeRadians(
                        directionRadians),
                    nameof(directionRadians));

#pragma warning disable CS0612, CS0618
            var wheels = _chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618

            // 没有舵轮时不能认为预对齐成功。
            if (wheels.Count == 0)
            {
                return false;
            }

            // 先检查所有舵轮能否到达目标机械角度。
            foreach (var wheel in wheels)
            {
                if (targetDegrees < wheel.AngleLowerLimit ||
                    targetDegrees > wheel.AngleUpperLimit)
                {
                    return false;
                }
            }

            // 模式切换前立即停止驱动轮。
            _chassis.PredefinedDriveStop();
            // 检查完成后再统一下发，避免只转动一部分舵轮。
            foreach (var wheel in wheels)
            {
                wheel.WriteAngle(targetDegrees);
            }

            return true;
        }

        /// <summary>
        /// 检查所有舵轮是否已经对准给定方向。
        /// </summary>
        public bool AreParallelWheelsAligned(
            double directionRadians,
            double toleranceRadians)
        {
            NumericGuard.EnsureFiniteNonNegative(
                toleranceRadians,
                nameof(toleranceRadians));

            EnsureBodyFrameIsActive();
            var targetDegrees =
                ConvertRadiansToSingleDegrees(
                    AngleMath.NormalizeRadians(
                        directionRadians),
                    nameof(directionRadians));

            var toleranceDegrees =
                ConvertRadiansToSingleDegrees(
                    toleranceRadians,
                    nameof(toleranceRadians));

#pragma warning disable CS0612, CS0618
            var wheels = _chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618

            foreach (var wheel in wheels)
            {
                var angleErrorDegrees = targetDegrees - wheel.ReadAngle();

                if (Math.Abs(angleErrorDegrees) >
                    toleranceDegrees)
                {
                    return false;
                }
            }

            return true;
        }

        /// <summary>
        /// 停车并将舵轮预对齐到原地自转方向。
        /// 返回是否成功生成舵轮目标。
        /// </summary>
        public bool PrepareSpin(
            TimeSpan? interval = null,
            double alignmentToleranceDegrees = 2.0)
        {
            NumericGuard.EnsureFiniteNonNegative(
                alignmentToleranceDegrees,
                nameof(alignmentToleranceDegrees));
            EnsureRepresentableAsSingle(
                alignmentToleranceDegrees,
                nameof(alignmentToleranceDegrees));

            EnsureBodyFrameIsActive();

            var success =
                _chassis.PrepareRotateWheels(
                    alignmentToleranceDegrees:
                        (float)alignmentToleranceDegrees);

            if (!success)
            {
                _chassis.PredefinedDriveStop();
            }
            return success;
        }

        /// <summary>
        /// 将已到位的自转舵角和轮速方向一次性交接给XYTh，
        /// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。
        /// </summary>
        public bool AdoptPreparedSpinForXYTh(
            double toleranceRadians =
                2.0 * Math.PI / 180.0)
        {
            NumericGuard.EnsureFiniteNonNegative(
                toleranceRadians,
                nameof(toleranceRadians));

            EnsureBodyFrameIsActive();

            var success =
                _chassis
                    .AdoptPreparedRotateWheelsForXYTh(
                        ConvertRadiansToSingleDegrees(
                            toleranceRadians,
                            nameof(toleranceRadians)));

            if (!success)
            {
                _chassis.PredefinedDriveStop();
            }

            return success;
        }
        /// <summary>
        /// 所有舵轮是否已对齐到原地自转方向。
        /// </summary>
        public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;



    }
}