// 将统一命令转换为原 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>
        /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
        /// </summary>
        private void EnsureBodyFrameIsActive()
        {
            var bias = _chassis.GetOriginBias();

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

            throw new InvalidOperationException(
                "MultiWheelChassis的坐标偏置在适配器创建后被修改。" +
                $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°。" +
                "请不要再调用DirectionAngle或SetOriginBias控制蟹行。");
        }
        /// <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()
        {
            _chassis.SetOriginBias(
                x: 0.0f,
                y: 0.0f,
                th: 0.0f);
        }
        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直接使用真实车体坐标系。
            ResetToBodyFrame();
        }


        /// <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>
        /// 按底盘减速度配置平滑停车，需要在控制周期中持续调用。
        /// </summary>
        public void RampStop(TimeSpan? interval = null)
        {
            _chassis.RampStop(interval);
        }
        /// <summary>
        /// 立即将所有驱动轮速度下发为零。
        /// </summary>
        public void StopImmediately()
        {
            _chassis.PredefinedDriveStop();
        }

        /// <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();
            _chassis.PredefinedDriveStop();

            var success =
                _chassis.SendRotateMotion(
                    0.0f,
                    interval);

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



    }
}