using System;

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)
        {
            EnsureFinitePositive(
                timeConstantSeconds,
                nameof(timeConstantSeconds));

            _timeConstantSeconds =
                timeConstantSeconds;
        }

        /// <summary>
        /// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
        /// </summary>
        public double TimeConstantSeconds =>
            _timeConstantSeconds;

        /// <summary>
        /// 获取滤波器是否已经接收过有效初值。
        /// </summary>
        public bool IsInitialized =>
            _isInitialized;

        /// <summary>
        /// 获取当前滤波输出；尚未初始化时读取会抛出异常。
        /// </summary>
        public double Value
        {
            get
            {
                if (!_isInitialized)
                {
                    throw new InvalidOperationException(
                        "一阶低通滤波器尚未初始化。");
                }

                return _value;
            }
        }

        /// <summary>
        /// 使用当前输入和真实采样间隔更新滤波结果。
        /// </summary>
        public double Update(
            double input,
            double deltaTimeSeconds)
        {
            EnsureFinite(
                input,
                nameof(input));
            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)
        {
            EnsureFinite(
                initialValue,
                nameof(initialValue));

            _value = initialValue;
            _isInitialized = true;
        }

        /// <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 EnsureFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "滤波输入必须是有限值。");
            }
        }
    }
}
