// Shared层底盘边界：对外使用SI单位，对内适配旧版MultiWheelChassis的混合单位接口。
using System;
using CommonUsage.Chassis;

namespace MyParking.Shared
{
    /// <summary>
    /// 将真实车体系刚体速度转换为旧版MultiWheelChassis命令，车体系约定为X向前、Y向左、逆时针为正。
    /// </summary>
    public sealed class MultiWheelChassisAdapter
    {
        #region 辅助内容

        // 旧底盘原点偏置使用float角度值，此容差用于判断坐标系是否已经切换到位。
        private const float BiasTolerance = 0.001f;

        // 小于该值的线速度或角速度视为零，避免在静止附近进入方向不确定的运动学分支。
        private const double MotionDeadband = 1e-6;

        private readonly MultiWheelChassis _chassis;

        // β：当前运动系X轴相对真实车体X轴的逆时针夹角，单位为rad。
        private double _activeMotionDirectionRadians;

        /// <summary>
        /// 当前适配器对应的车辆编号。
        /// </summary>
        public int VehicleId { get; }

        /// <summary>
        /// 车体原点到最远舵轮中心的距离，单位为m，用于描述底盘整体外接半径。
        /// </summary>
        public double MaximumWheelRadiusMeters { get; }

        /// <summary>
        /// 车体原点到最前或最后舵轮中心的最大纵向距离，单位为m；对称四舵轮底盘中通常为轴距的一半。
        /// </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>
        /// 舵角误差高斯降速门控的宽度，单位为deg；数值越小，舵轮未对齐时驱动降速越明显。
        /// </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>
        /// 检查旧底盘是否处于指定β运动坐标系，防止准备状态与当前命令使用的坐标系不一致。
        /// </summary>
        /// <param name="motionDirectionRadians">运动系X轴在真实车体系中的方向，单位为rad。</param>
        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运动坐标系；调用前必须停车并完成该方向的舵轮预对齐。
        /// </summary>
        /// <param name="motionDirectionRadians">运动系X轴在真实车体系中的方向，单位为rad；0为车头，π/2为车体左侧。</param>
        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;
            }

            // SetOriginBias会把每个真实轮位重新表达在运动系中，并同步设置舵角零方向；
            // 车辆本体没有发生虚拟旋转，后续SendMotion仍使用这些真实轮位完成四轮解算。
            _chassis.SetOriginBias(
                x: 0.0f,
                y: 0.0f,
                th: biasDegrees);
            SetActiveMotionDirection(
                normalizedDirectionRadians);
        }

        /// <summary>
        /// 缓存当前运动坐标系方向，供控制周期内转换车体速度。
        /// </summary>
        private void SetActiveMotionDirection(
            double motionDirectionRadians)
        {
            _activeMotionDirectionRadians =
                AngleMath.NormalizeRadians(
                    motionDirectionRadians);
        }

        /// <summary>
        /// 创建旧版底盘的SI单位适配器，并从真实轮位提取车辆几何尺寸。
        /// </summary>
        /// <param name="chassis">已经完成舵轮初始化的旧版多舵轮底盘。</param>
        /// <param name="vehicleId">正整数车辆编号，仅标识该适配器所属车辆。</param>
        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尚未完成舵轮初始化，" +
                    "不能创建底盘适配器。");
            }
            // 几何尺寸必须取PhysicalPosition，避免受旧底盘当前原点偏置和运动坐标系影响。
            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>
        /// <param name="bodyTwist">真实车体系速度，线速度单位为m/s，角速度单位为rad/s。</param>
        /// <param name="interval">与上一条底盘命令的实际时间间隔，用于旧底盘速度和舵角变化率处理。</param>
        /// <returns>旧底盘是否成功接受并完成运动分解。</returns>
        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);

            // SendMotion用速度符号表达前进/倒车，而GCP角度始终相对当前行驶方向计算。
            var signedCenterSpeedMetersPerSecond =
                travelDirection *
                linearSpeedMetersPerSecond;

            // 刚体速度关系v(point)=v(center)+ω×r；前后GCP位于运动系X轴的±ControlPointRadius处。
            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>
        /// <param name="directionRadians">舵轮相对真实车体X轴的目标方向，单位为rad。</param>
        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;
    }
}