using System;
using System.Diagnostics;
using ClumsyCore.Interfaces;
using MyParking.Shared;

namespace MultiWheelC.StateEstimation
{
    /// <summary>
    /// 读取Detour位姿，忽略重复或明显异常的观测，并估算车辆二维速度。
    /// </summary>
    public sealed class DetourVehicleStateProvider
        : IVehicleStateProvider
    {
        public const double DefaultMaximumLinearSpeedMetersPerSecond =
            1.20;
        public const double DefaultMaximumAngularSpeedRadiansPerSecond =
            Math.PI / 4.0;
        public const double DefaultPositionJumpMarginMeters =
            0.03;
        public const double DefaultHeadingJumpMarginRadians =
            5.0 * Math.PI / 180.0;
        public const double DefaultVelocityPositionResidualMeters =
            0.04;
        public const double DefaultVelocityHeadingResidualRadians =
            5.0 * Math.PI / 180.0;
        public const double DefaultStationaryConfirmationSeconds =
            0.35;

        private const double MillimetersPerMeter = 1000.0;
        private const double PositionEqualityToleranceMeters = 1e-9;
        private const double HeadingEqualityToleranceRadians = 1e-8;

        private readonly object _syncRoot = new object();
        private readonly Stopwatch _clock = Stopwatch.StartNew();
        private readonly VelocityEstimator2D _velocityEstimator;
        private readonly double _maximumLinearSpeedMetersPerSecond;
        private readonly double _maximumAngularSpeedRadiansPerSecond;
        private readonly double _positionJumpMarginMeters;
        private readonly double _headingJumpMarginRadians;
        private readonly double _velocityPositionResidualMeters;
        private readonly double _velocityHeadingResidualRadians;
        private readonly double _stationaryConfirmationSeconds;

        private bool _hasAcceptedPose;
        private Pose2D _acceptedPoseInWorld;
        private double _acceptedTimestampSeconds;
        private VehicleState _latestState;
        private bool _stationaryHoldActive;

        /// <summary>
        /// 创建使用停车机器人默认物理边界和速度滤波参数的Detour状态源。
        /// </summary>
        public DetourVehicleStateProvider()
            : this(
                new VelocityEstimator2D(),
                DefaultMaximumLinearSpeedMetersPerSecond,
                DefaultMaximumAngularSpeedRadiansPerSecond,
                DefaultPositionJumpMarginMeters,
                DefaultHeadingJumpMarginRadians,
                DefaultVelocityPositionResidualMeters,
                DefaultVelocityHeadingResidualRadians,
                DefaultStationaryConfirmationSeconds)
        {
        }

        /// <summary>
        /// 创建使用指定物理边界、静止确认时间和速度估计器的Detour状态源。
        /// </summary>
        public DetourVehicleStateProvider(
            VelocityEstimator2D velocityEstimator,
            double maximumLinearSpeedMetersPerSecond,
            double maximumAngularSpeedRadiansPerSecond,
            double positionJumpMarginMeters,
            double headingJumpMarginRadians,
            double velocityPositionResidualMeters,
            double velocityHeadingResidualRadians,
            double stationaryConfirmationSeconds)
        {
            _velocityEstimator = velocityEstimator ??
                throw new ArgumentNullException(
                    nameof(velocityEstimator));

            EnsureFinitePositive(
                maximumLinearSpeedMetersPerSecond,
                nameof(maximumLinearSpeedMetersPerSecond));
            EnsureFinitePositive(
                maximumAngularSpeedRadiansPerSecond,
                nameof(maximumAngularSpeedRadiansPerSecond));
            EnsureFiniteNonNegative(
                positionJumpMarginMeters,
                nameof(positionJumpMarginMeters));
            EnsureFiniteNonNegative(
                headingJumpMarginRadians,
                nameof(headingJumpMarginRadians));
            EnsureFinitePositive(
                velocityPositionResidualMeters,
                nameof(velocityPositionResidualMeters));
            EnsureFinitePositive(
                velocityHeadingResidualRadians,
                nameof(velocityHeadingResidualRadians));
            EnsureFinitePositive(
                stationaryConfirmationSeconds,
                nameof(stationaryConfirmationSeconds));

            _maximumLinearSpeedMetersPerSecond =
                maximumLinearSpeedMetersPerSecond;
            _maximumAngularSpeedRadiansPerSecond =
                maximumAngularSpeedRadiansPerSecond;
            _positionJumpMarginMeters =
                positionJumpMarginMeters;
            _headingJumpMarginRadians =
                headingJumpMarginRadians;
            _velocityPositionResidualMeters =
                velocityPositionResidualMeters;
            _velocityHeadingResidualRadians =
                velocityHeadingResidualRadians;
            _stationaryConfirmationSeconds =
                stationaryConfirmationSeconds;
        }

        /// <summary>
        /// 获取最近一次读取失败或异常观测被忽略的原因，正常时为空字符串。
        /// </summary>
        public string LastFailureReason { get; private set; } = "";

        /// <summary>
        /// 尝试读取Detour；重复帧保留最近状态，明显异常帧只忽略本次观测。
        /// </summary>
        public bool TryGetState(out VehicleState state)
        {
            lock (_syncRoot)
            {
                try
                {
                    var poseInWorld =
                        ReadDetourPoseInWorld();
                    var timestampSeconds =
                        _clock.Elapsed.TotalSeconds;

                    if (!_hasAcceptedPose)
                    {
                        state = AcceptPoseAfterReset(
                            poseInWorld,
                            timestampSeconds);
                        LastFailureReason = "";
                        return true;
                    }

                    if (ArePosesEquivalent(
                            poseInWorld,
                            _acceptedPoseInWorld))
                    {
                        state = HandleRepeatedPose(
                            timestampSeconds);
                        LastFailureReason = "";
                        return true;
                    }

                    // 静止保持后出现新定位时重新建立差分基准，
                    // 避免用很长的静止时间稀释第一次运动速度。
                    if (_stationaryHoldActive)
                    {
                        state = AcceptPoseAfterReset(
                            poseInWorld,
                            timestampSeconds);
                        LastFailureReason = "";
                        return true;
                    }

                    var elapsedSeconds =
                        timestampSeconds -
                        _acceptedTimestampSeconds;

                    if (!IsMotionPlausible(
                            _acceptedPoseInWorld,
                            poseInWorld,
                            elapsedSeconds))
                    {
                        // 单帧异常不进入差分器，也不中断调用方；下一次
                        // 正常观测仍相对最近有效位姿和真实时间差计算。
                        state = _latestState;
                        LastFailureReason =
                            "Detour位姿变化超过车辆绝对运动边界，本次观测已忽略。";
                        return true;
                    }

                    if (IsVelocityInnovationAbnormal(
                            poseInWorld,
                            elapsedSeconds))
                    {
                        state = AcceptPoseAfterVelocityRebase(
                            poseInWorld,
                            timestampSeconds);
                        LastFailureReason =
                            "Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。";
                        return true;
                    }

                    state = AcceptContinuousPose(
                        poseInWorld,
                        timestampSeconds);
                    LastFailureReason = "";
                    return true;
                }
                catch (Exception exception)
                {
                    state = default;
                    LastFailureReason =
                        "Detour车辆状态读取失败：" +
                        exception.Message;
                    return false;
                }
            }
        }

        /// <summary>
        /// 清除Detour位姿历史和速度估计状态。
        /// </summary>
        public void Reset()
        {
            lock (_syncRoot)
            {
                _velocityEstimator.Reset();
                _hasAcceptedPose = false;
                _acceptedPoseInWorld = Pose2D.Identity;
                _acceptedTimestampSeconds = 0.0;
                _latestState = default;
                _stationaryHoldActive = false;
                LastFailureReason = "";
            }
        }

        /// <summary>
        /// 读取Detour毫米和角度数据并转换为世界坐标SI位姿。
        /// </summary>
        private static Pose2D ReadDetourPoseInWorld()
        {
            var location =
                DetourInterface.getCartLocation();

            EnsureFinite(location.x, "DetourX");
            EnsureFinite(location.y, "DetourY");
            EnsureFinite(location.th, "DetourTheta");

            return new Pose2D(
                location.x / MillimetersPerMeter,
                location.y / MillimetersPerMeter,
                AngleMath.NormalizeRadians(
                    AngleMath.DegreesToRadians(
                        location.th)));
        }

        /// <summary>
        /// 接受连续有效定位并更新速度估计和差分基准。
        /// </summary>
        private VehicleState AcceptContinuousPose(
            Pose2D poseInWorld,
            double timestampSeconds)
        {
            _latestState =
                _velocityEstimator.Update(
                    poseInWorld,
                    timestampSeconds);
            _acceptedPoseInWorld = poseInWorld;
            _acceptedTimestampSeconds =
                timestampSeconds;
            _stationaryHoldActive = false;
            return _latestState;
        }

        /// <summary>
        /// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。
        /// </summary>
        private VehicleState AcceptPoseAfterVelocityRebase(
            Pose2D poseInWorld,
            double timestampSeconds)
        {
            _latestState =
                _velocityEstimator
                    .RebasePreservingVelocity(
                        poseInWorld,
                        timestampSeconds);
            _acceptedPoseInWorld = poseInWorld;
            _acceptedTimestampSeconds =
                timestampSeconds;
            _stationaryHoldActive = false;
            return _latestState;
        }

        /// <summary>
        /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
        /// </summary>
        private VehicleState AcceptPoseAfterReset(
            Pose2D poseInWorld,
            double timestampSeconds)
        {
            _latestState =
                _velocityEstimator.Reset(
                    poseInWorld,
                    timestampSeconds);
            _acceptedPoseInWorld = poseInWorld;
            _acceptedTimestampSeconds =
                timestampSeconds;
            _hasAcceptedPose = true;
            _stationaryHoldActive = false;
            return _latestState;
        }

        /// <summary>
        /// 对重复Detour观测保留最近状态，并在持续不变后将速度归零。
        /// </summary>
        private VehicleState HandleRepeatedPose(
            double timestampSeconds)
        {
            var unchangedSeconds =
                timestampSeconds -
                _acceptedTimestampSeconds;

            if (!_stationaryHoldActive &&
                unchangedSeconds >=
                _stationaryConfirmationSeconds)
            {
                _latestState =
                    new VehicleState(
                        timestampSeconds,
                        _acceptedPoseInWorld,
                        Twist2D.Zero,
                        true);
                _stationaryHoldActive = true;
            }

            return _latestState;
        }

        /// <summary>
        /// 判断两次有效Detour观测之间的变化是否超过车辆绝对运动能力。
        /// </summary>
        private bool IsMotionPlausible(
            Pose2D startPoseInWorld,
            Pose2D endPoseInWorld,
            double deltaTimeSeconds)
        {
            if (!IsFinite(deltaTimeSeconds) ||
                deltaTimeSeconds <= 0.0)
            {
                return false;
            }

            var deltaX =
                endPoseInWorld.XMeters -
                startPoseInWorld.XMeters;
            var deltaY =
                endPoseInWorld.YMeters -
                startPoseInWorld.YMeters;
            var displacementMeters =
                Math.Sqrt(
                    deltaX * deltaX +
                    deltaY * deltaY);
            var headingChangeRadians =
                Math.Abs(
                    AngleMath.ShortestDifferenceRadians(
                        endPoseInWorld.YawRadians,
                        startPoseInWorld.YawRadians));

            var maximumDisplacementMeters =
                _maximumLinearSpeedMetersPerSecond *
                deltaTimeSeconds +
                _positionJumpMarginMeters;
            var maximumHeadingChangeRadians =
                _maximumAngularSpeedRadiansPerSecond *
                deltaTimeSeconds +
                _headingJumpMarginRadians;

            return displacementMeters <=
                       maximumDisplacementMeters &&
                   headingChangeRadians <=
                       maximumHeadingChangeRadians;
        }

        /// <summary>
        /// 判断新位姿是否明显偏离上一滤波速度给出的恒速预测。
        /// </summary>
        private bool IsVelocityInnovationAbnormal(
            Pose2D poseInWorld,
            double deltaTimeSeconds)
        {
            if (!_latestState.HasValidVelocityEstimate)
            {
                return false;
            }

            var predictedX =
                _acceptedPoseInWorld.XMeters +
                _latestState.TwistInWorld
                    .VxMetersPerSecond *
                deltaTimeSeconds;
            var predictedY =
                _acceptedPoseInWorld.YMeters +
                _latestState.TwistInWorld
                    .VyMetersPerSecond *
                deltaTimeSeconds;
            var predictedYaw =
                AngleMath.NormalizeRadians(
                    _acceptedPoseInWorld.YawRadians +
                    _latestState.TwistInWorld
                        .OmegaRadiansPerSecond *
                    deltaTimeSeconds);

            var positionResidualX =
                poseInWorld.XMeters - predictedX;
            var positionResidualY =
                poseInWorld.YMeters - predictedY;
            var positionResidualMeters =
                Math.Sqrt(
                    positionResidualX * positionResidualX +
                    positionResidualY * positionResidualY);
            var headingResidualRadians =
                Math.Abs(
                    AngleMath.ShortestDifferenceRadians(
                        poseInWorld.YawRadians,
                        predictedYaw));

            return positionResidualMeters >
                       _velocityPositionResidualMeters ||
                   headingResidualRadians >
                       _velocityHeadingResidualRadians;
        }

        /// <summary>
        /// 判断两次读取是否为Detour保持输出的同一数值帧。
        /// </summary>
        private static bool ArePosesEquivalent(
            Pose2D firstPose,
            Pose2D secondPose)
        {
            return Math.Abs(
                       firstPose.XMeters -
                       secondPose.XMeters) <=
                   PositionEqualityToleranceMeters &&
                   Math.Abs(
                       firstPose.YMeters -
                       secondPose.YMeters) <=
                   PositionEqualityToleranceMeters &&
                   Math.Abs(
                       AngleMath.ShortestDifferenceRadians(
                           firstPose.YawRadians,
                           secondPose.YawRadians)) <=
                   HeadingEqualityToleranceRadians;
        }

        /// <summary>
        /// 检查数值是否为正有限值。
        /// </summary>
        private static void EnsureFinitePositive(
            double value,
            string parameterName)
        {
            EnsureFinite(value, parameterName);

            if (value <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "状态源参数必须是正有限值。");
            }
        }

        /// <summary>
        /// 检查数值是否为非负有限值。
        /// </summary>
        private static void EnsureFiniteNonNegative(
            double value,
            string parameterName)
        {
            EnsureFinite(value, parameterName);

            if (value < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "状态源参数必须是非负有限值。");
            }
        }

        /// <summary>
        /// 检查数值是否为有限值。
        /// </summary>
        private static void EnsureFinite(
            double value,
            string parameterName)
        {
            if (!IsFinite(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "状态源参数和Detour位姿必须是有限值。");
            }
        }

        /// <summary>
        /// 判断数值是否可用于状态估计。
        /// </summary>
        private static bool IsFinite(double value)
        {
            return !double.IsNaN(value) &&
                   !double.IsInfinity(value);
        }
    }
}