using MyParking.Shared;

namespace MultiWheelC.StateEstimation
{
    /// <summary>
    /// 使用真实采样时间间隔对单个连续量执行在线一阶低通滤波。
    /// </summary>
    public sealed class FirstOrderLowPassFilter
    {
        private readonly double _timeConstantSeconds;
        private bool _isInitialized;
        private double _value;

        /// <summary>
        /// 创建使用指定时间常数的一阶低通滤波器。
        /// </summary>
        public FirstOrderLowPassFilter(
            double timeConstantSeconds)
        {
            NumericGuard.EnsureFinitePositive(
                timeConstantSeconds,
                nameof(timeConstantSeconds));

            _timeConstantSeconds =
                timeConstantSeconds;
        }

        /// <summary>
        /// 使用当前输入和真实采样间隔更新滤波结果。
        /// </summary>
        public double Update(
            double input,
            double deltaTimeSeconds)
        {
            NumericGuard.EnsureFinite(
                input,
                nameof(input));
            NumericGuard.EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));

            if (!_isInitialized)
            {
                _value = input;
                _isInitialized = true;
                return _value;
            }

            var alpha =
                deltaTimeSeconds /
                (_timeConstantSeconds +
                 deltaTimeSeconds);

            _value += alpha * (input - _value);
            return _value;
        }

        /// <summary>
        /// 清除历史输出，使下一次有效输入直接成为新的初值。
        /// </summary>
        public void Reset()
        {
            _value = 0.0;
            _isInitialized = false;
        }

        /// <summary>
        /// 将滤波器立即重置到指定的有限初值。
        /// </summary>
        public void Reset(double initialValue)
        {
            NumericGuard.EnsureFinite(
                initialValue,
                nameof(initialValue));

            _value = initialValue;
            _isInitialized = true;
        }

    }
}
