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

namespace MyParking.Shared
{
    /// <summary>
    /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
    /// 车体坐标系固定为X向前、Y向左、逆时针为正。
    /// </summary>
    public sealed class MultiWheelChassisAdapter
    {
        #region 辅助内容
        private const double RadiansToDegrees = 180.0 / Math.PI;
        private const float BiasTolerance = 0.001f;
        private readonly MultiWheelChassis _chassis;
        /// <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>
        /// Width of the steering-alignment speed gate, in degrees.
        /// </summary>
        public double SteeringAlignmentSigmaDegrees
        {
            get => _chassis.SteeringAlignmentSigmaDegrees;
            set
            {
                if (double.IsNaN(value) ||
                    double.IsInfinity(value) ||
                    value <= 0.0 ||
                    value > float.MaxValue)
                {
                    throw new ArgumentOutOfRangeException(
                        nameof(value),
                        "Steering alignment sigma must be a positive finite value.");
                }

                _chassis.SteeringAlignmentSigmaDegrees =
                    (float)value;
            }
        }

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

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

            var expectedBiasDegrees =
                (float)(
                    -FrameTransform2D.NormalizeAngle(
                        motionDirectionRadians) *
                    RadiansToDegrees);
            var bias = _chassis.GetOriginBias();
            var angleErrorDegrees =
                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>
        /// 将角度归一化到[-180°,180°]附近。
        /// </summary>
        private static float NormalizeDegrees(float degrees)
        {
            return (float)(
                degrees -
                Math.Round(degrees / 360.0) * 360.0);
        }
        /// <summary>
        /// 检查底盘命令是否包含无效数值。
        /// </summary>
        private static void ValidateTwist(Twist2D twist)
        {
            ValidateFinite(
                twist.VxMetersPerSecond,
                nameof(twist.VxMetersPerSecond));

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

            ValidateFinite(
                twist.OmegaRadiansPerSecond,
                nameof(twist.OmegaRadiansPerSecond));
        }
        /// <summary>
        /// 检查数值是否为有限值。
        /// </summary>
        private static void ValidateFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "底盘速度命令不能是NaN或无穷大。");
            }

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

        /// <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)
        {
            ValidateFinite(
                motionDirectionRadians,
                nameof(motionDirectionRadians));

            var biasDegrees =
                (float)(
                    -FrameTransform2D.NormalizeAngle(
                        motionDirectionRadians) *
                    RadiansToDegrees);
            var currentBias =
                _chassis.GetOriginBias();

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

            _chassis.SetOriginBias(
                x: 0.0f,
                y: 0.0f,
                th: biasDegrees);
        }
        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)
            {
                throw new InvalidOperationException(
                    "Wheel positions cannot produce valid chassis dimensions.");
            }

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

        /// <summary>
        /// 将车体坐标系速度命令发送给多舵轮底盘。
        /// </summary>
        public bool Send(ChassisCommand command, TimeSpan? interval = null)
        {
            if (command.VehicleId != VehicleId)
            {
                throw new InvalidOperationException(
                    $"命令车辆编号{command.VehicleId}与适配器车辆编号" +
                    $"{VehicleId}不一致。");
            }
            ValidateTwist(command.BodyTwist);
            // 防止其他旧逻辑再次调用DirectionAngle或
            // SetOriginBias改变底盘坐标语义。
            EnsureBodyFrameIsActive();
            var vxMetersPerSecond =
                            (float)command.BodyTwist.VxMetersPerSecond;
            var vyMetersPerSecond =
                (float)command.BodyTwist.VyMetersPerSecond;
            var omegaDegreesPerSecond =
                (float)(
                    command.BodyTwist.OmegaRadiansPerSecond *
                    RadiansToDegrees);
            var success = _chassis.SendXYThSpeed(
                                vxMetersPerSecond,
                                vyMetersPerSecond,
                                omegaDegreesPerSecond,
                                interval);
            if (!success)
            {
                // 防止分解失败后继续执行上一条运动命令。
                _chassis.PredefinedDriveStop();
            }
            return success;
        }


        /// <summary>
        /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
        /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
        /// </summary>
        public bool SendVirtualAckermannMotion(
            double motionDirectionRadians,
            double speedMetersPerSecond,
            double steeringRadians,
            TimeSpan? interval = null)
        {
            ValidateFinite(
                motionDirectionRadians,
                nameof(motionDirectionRadians));
            ValidateFinite(
                speedMetersPerSecond,
                nameof(speedMetersPerSecond));
            ValidateFinite(
                steeringRadians,
                nameof(steeringRadians));
            EnsureMotionFrameIsActive(
                motionDirectionRadians);

            if (Math.Abs(steeringRadians) >=
                Math.PI / 2.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(steeringRadians),
                    "虚拟阿克曼转向角必须位于正负90度以内。");
            }

            var steeringDegrees =
                (float)(
                    steeringRadians *
                    RadiansToDegrees);
            var success =
                _chassis.SendMotion(
                    (float)speedMetersPerSecond,
                    steeringDegrees,
                    -steeringDegrees,
                    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 = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
                                    RadiansToDegrees);

#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)
        {
            if (double.IsNaN(toleranceRadians) ||
        double.IsInfinity(toleranceRadians) ||
        toleranceRadians < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(toleranceRadians),
                    "舵轮到位容差必须是非负有限值。");
            }

            EnsureBodyFrameIsActive();
            var targetDegrees = (float)(
                FrameTransform2D.NormalizeAngle(directionRadians) *
                180.0 / Math.PI);

            var toleranceDegrees = (float)(
                Math.Abs(toleranceRadians) *
                180.0 / Math.PI);

#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)
        {
            EnsureBodyFrameIsActive();

            var success =
                _chassis.PrepareRotateWheels(
                    alignmentToleranceDegrees: 2.0f);

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

        /// <summary>
        /// 将已到位的自转舵角和轮速方向一次性交接给XYTh，
        /// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。
        /// </summary>
        public bool AdoptPreparedSpinForXYTh(
            double toleranceRadians =
                2.0 * Math.PI / 180.0)
        {
            if (double.IsNaN(toleranceRadians) ||
                double.IsInfinity(toleranceRadians) ||
                toleranceRadians < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(toleranceRadians),
                    "自转状态交接容差必须是非负有限值。");
            }

            EnsureBodyFrameIsActive();

            var success =
                _chassis
                    .AdoptPreparedRotateWheelsForXYTh(
                        (float)(
                            toleranceRadians *
                            RadiansToDegrees));

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

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



    }
}