roplat 0.2.0

use std::collections::VecDeque;

use num_traits::Float;

use crate::Node;
use crate::error::RoplatError;

// ==================== 移动平均滤波器 ====================

/// 移动平均滤波器 (FIR 滤波器的一种)
///
/// 在滑动窗口内计算输入数据的平均值，用于平滑噪声数据
pub struct MovingAverage<T>
where
    T: Float + Send + Sync,
{
    window_size: usize,
    buffer: VecDeque<T>,
}

impl<T> MovingAverage<T>
where
    T: Float + Send + Sync,
{
    /// 创建移动平均滤波器。
    pub fn new(window_size: usize) -> Self {
        if window_size == 0 {
            panic!("窗口大小必须大于0");
        }
        Self { window_size, buffer: VecDeque::with_capacity(window_size) }
    }
}

impl<T> Node for MovingAverage<T>
where
    T: Float + Send + Sync + 'static,
{
    type Input = T;
    type Output = Result<T, RoplatError>;
    type Error = RoplatError;

    async fn process(&mut self, input: T) -> Result<T, RoplatError> {
        // 1. 维护滑动窗口
        if self.buffer.len() >= self.window_size {
            self.buffer.pop_front();
        }
        self.buffer.push_back(input);

        // 2. 计算平均值
        let sum = self.buffer.iter().fold(T::zero(), |acc, &x| acc + x);
        let count = T::from(self.buffer.len()).unwrap();

        Ok(sum / count)
    }
}

// ==================== 指数移动平均滤波器 (IIR) ====================

/// 指数移动平均滤波器 (低通滤波器)
///
/// output = alpha * input + (1 - alpha) * last_output
/// alpha 越小，平滑效果越强，但响应越慢
/// 典型值：alpha = 2 / (N + 1)，其中 N 是等效窗口大小
pub struct ExponentialMovingAverage<T>
where
    T: Float + Send + Sync,
{
    alpha: T,
    last_output: Option<T>,
}

impl<T> ExponentialMovingAverage<T>
where
    T: Float + Send + Sync,
{
    /// 创建指数移动平均滤波器。
    pub fn new(alpha: T) -> Self {
        if alpha < T::zero() || alpha > T::one() {
            panic!("alpha 必须在 [0, 1] 范围内");
        }
        Self { alpha, last_output: None }
    }

    /// 从等效窗口大小创建
    /// N 越大，平滑效果越强
    pub fn from_window_size(n: usize) -> Self
    where
        T: Float + num_traits::NumCast,
    {
        let n_float = T::from(n).unwrap();
        let two = T::from(2usize).unwrap();
        let alpha = two / (n_float + T::one());
        Self::new(alpha)
    }
}

impl<T> Node for ExponentialMovingAverage<T>
where
    T: Float + Send + Sync + 'static,
{
    type Input = T;
    type Output = Result<T, RoplatError>;
    type Error = RoplatError;

    async fn process(&mut self, input: T) -> Result<T, RoplatError> {
        let output = match self.last_output {
            Some(last) => self.alpha * input + (T::one() - self.alpha) * last,
            None => input, // 第一个输入直接输出
        };

        self.last_output = Some(output);
        Ok(output)
    }
}

// ==================== 中值滤波器 ====================

/// 中值滤波器
///
/// 在滑动窗口内取中值，用于消除脉冲噪声
pub struct MedianFilter<T>
where
    T: Float + Send + Sync,
{
    window_size: usize,
    buffer: VecDeque<T>,
}

impl<T> MedianFilter<T>
where
    T: Float + Send + Sync,
{
    /// 创建中值滤波器。
    pub fn new(window_size: usize) -> Self {
        if window_size == 0 {
            panic!("窗口大小必须大于0");
        }
        if window_size.is_multiple_of(2) {
            panic!("窗口大小应该是奇数，以便计算中值");
        }
        Self { window_size, buffer: VecDeque::with_capacity(window_size) }
    }
}

impl<T> Node for MedianFilter<T>
where
    T: Float + Send + Sync + 'static,
{
    type Input = T;
    type Output = Result<T, RoplatError>;
    type Error = RoplatError;

    async fn process(&mut self, input: T) -> Result<T, RoplatError> {
        // 1. 维护滑动窗口
        if self.buffer.len() >= self.window_size {
            self.buffer.pop_front();
        }
        self.buffer.push_back(input);

        // 2. 计算中值
        let mut sorted: Vec<T> = self.buffer.iter().copied().collect();
        sorted.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));

        let mid = sorted.len() / 2;
        Ok(sorted[mid])
    }
}

// ==================== 限幅滤波器 ====================

/// 限幅滤波器
///
/// 限制输出的变化幅度，用于抑制突变噪声
/// 如果当前输入与上一次输出的差值超过阈值，则限制在阈值内
pub struct RateLimiter<T>
where
    T: Float + Send + Sync,
{
    max_change: T,
    last_output: Option<T>,
}

impl<T> RateLimiter<T>
where
    T: Float + Send + Sync,
{
    /// 创建限幅滤波器。
    pub fn new(max_change: T) -> Self {
        Self { max_change, last_output: None }
    }
}

impl<T> Node for RateLimiter<T>
where
    T: Float + Send + Sync + 'static,
{
    type Input = T;
    type Output = Result<T, RoplatError>;
    type Error = RoplatError;

    async fn process(&mut self, input: T) -> Result<T, RoplatError> {
        let output = match self.last_output {
            Some(last) => {
                let change = input - last;
                if change > self.max_change {
                    last + self.max_change
                } else if change < -self.max_change {
                    last - self.max_change
                } else {
                    input
                }
            }
            None => input,
        };

        self.last_output = Some(output);
        Ok(output)
    }
}

// ==================== 死区滤波器 ====================

/// 死区滤波器
///
/// 当输入变化小于阈值时，输出保持不变
/// 用于消除小幅波动噪声
pub struct DeadzoneFilter<T>
where
    T: Float + Send + Sync,
{
    threshold: T,
    last_output: Option<T>,
}

impl<T> DeadzoneFilter<T>
where
    T: Float + Send + Sync,
{
    /// 创建死区滤波器。
    pub fn new(threshold: T) -> Self {
        Self { threshold, last_output: None }
    }
}

impl<T> Node for DeadzoneFilter<T>
where
    T: Float + Send + Sync + 'static,
{
    type Input = T;
    type Output = Result<T, RoplatError>;
    type Error = RoplatError;

    async fn process(&mut self, input: T) -> Result<T, RoplatError> {
        let output = match self.last_output {
            Some(last) => {
                let change = (input - last).abs();
                if change < self.threshold {
                    last // 变化太小，保持输出不变
                } else {
                    input
                }
            }
            None => input,
        };

        self.last_output = Some(output);
        Ok(output)
    }
}

// ==================== 低通滤波器 (一阶滞后) ====================

/// 一阶滞后滤波器 (低通滤波器)
///
/// 与指数移动平均类似，但使用时间常数参数
/// output = last_output + (input - last_output) / (time_constant + 1)
/// time_constant 越大，滤波效果越强
pub struct LowPassFilter<T>
where
    T: Float + Send + Sync,
{
    time_constant: T,
    last_output: Option<T>,
}

impl<T> LowPassFilter<T>
where
    T: Float + Send + Sync,
{
    /// 创建一阶低通滤波器。
    pub fn new(time_constant: T) -> Self {
        if time_constant < T::zero() {
            panic!("时间常数必须大于等于0");
        }
        Self { time_constant, last_output: None }
    }
}

impl<T> Node for LowPassFilter<T>
where
    T: Float + Send + Sync + 'static,
{
    type Input = T;
    type Output = Result<T, RoplatError>;
    type Error = RoplatError;

    async fn process(&mut self, input: T) -> Result<T, RoplatError> {
        let output = match self.last_output {
            Some(last) => {
                let delta = (input - last) / (self.time_constant + T::one());
                last + delta
            }
            None => input,
        };

        self.last_output = Some(output);
        Ok(output)
    }
}

// ==================== 测试 ====================

#[cfg(test)]
mod tests {
    use super::*;

    // ==================== 移动平均滤波器测试 ====================

    #[tokio::test]
    async fn test_moving_average_basic() {
        let mut filter = MovingAverage::<f64>::new(3);

        // 第一个值: [10], avg = 10
        let result = filter.process(10.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);

        // 第二个值: [10, 20], avg = 15
        let result = filter.process(20.0).await.unwrap();
        assert!((result - 15.0).abs() < 1e-10);

        // 第三个值: [10, 20, 30], avg = 20
        let result = filter.process(30.0).await.unwrap();
        assert!((result - 20.0).abs() < 1e-10);

        // 第四个值: [20, 30, 40], avg = 30 (10 被挤出窗口)
        let result = filter.process(40.0).await.unwrap();
        assert!((result - 30.0).abs() < 1e-10);
    }

    #[tokio::test]
    async fn test_moving_average_smoothing() {
        let mut filter = MovingAverage::<f64>::new(5);

        // 输入带噪声的数据
        let noisy_data = vec![10.0, 12.0, 8.0, 11.0, 9.0, 10.0, 10.0, 10.0];
        for value in noisy_data {
            filter.process(value).await.unwrap();
        }

        // 最后几个值应该接近 10.0
        let result = filter.process(10.0).await.unwrap();
        assert!((result - 10.0).abs() < 1.0);
    }

    #[tokio::test]
    #[should_panic(expected = "窗口大小必须大于0")]
    async fn test_moving_average_zero_window() {
        MovingAverage::<f64>::new(0);
    }

    // ==================== 指数移动平均滤波器测试 ====================

    #[tokio::test]
    async fn test_exponential_moving_average_basic() {
        let mut filter = ExponentialMovingAverage::<f64>::new(0.5);

        // 第一个值直接输出
        let result = filter.process(10.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);

        // 第二个值: 0.5 * 20 + 0.5 * 10 = 15
        let result = filter.process(20.0).await.unwrap();
        assert!((result - 15.0).abs() < 1e-10);

        // 第三个值: 0.5 * 30 + 0.5 * 15 = 22.5
        let result = filter.process(30.0).await.unwrap();
        assert!((result - 22.5).abs() < 1e-10);
    }

    #[tokio::test]
    async fn test_exponential_moving_average_from_window() {
        let mut filter = ExponentialMovingAverage::<f64>::from_window_size(9);

        // alpha = 2 / (9 + 1) = 0.2
        filter.process(10.0).await.unwrap();

        // output = 0.2 * 20 + 0.8 * 10 = 4 + 8 = 12
        let result = filter.process(20.0).await.unwrap();
        assert!((result - 12.0).abs() < 0.1);

        // output = 0.2 * 30 + 0.8 * 12 = 6 + 9.6 = 15.6
        let result = filter.process(30.0).await.unwrap();
        assert!((result - 15.6).abs() < 0.1);
    }

    #[tokio::test]
    #[should_panic(expected = "alpha 必须在 [0, 1] 范围内")]
    async fn test_exponential_moving_average_invalid_alpha_low() {
        ExponentialMovingAverage::<f64>::new(-0.1);
    }

    #[tokio::test]
    #[should_panic(expected = "alpha 必须在 [0, 1] 范围内")]
    async fn test_exponential_moving_average_invalid_alpha_high() {
        ExponentialMovingAverage::<f64>::new(1.5);
    }

    #[tokio::test]
    async fn test_exponential_moving_average_boundary() {
        // alpha = 0.0: 不更新（但第一个值直接输出）
        let mut filter = ExponentialMovingAverage::<f64>::new(0.0);
        filter.process(10.0).await.unwrap();
        let result = filter.process(100.0).await.unwrap();
        // alpha = 0 时，output = 0 * input + 1 * last_output = last_output
        assert!((result - 10.0).abs() < 1e-10);

        // alpha = 1.0: 完全跟随输入
        let mut filter = ExponentialMovingAverage::<f64>::new(1.0);
        filter.process(10.0).await.unwrap();
        let result = filter.process(100.0).await.unwrap();
        assert!((result - 100.0).abs() < 1e-10);
    }

    // ==================== 中值滤波器测试 ====================

    #[tokio::test]
    async fn test_median_filter_basic() {
        let mut filter = MedianFilter::<f64>::new(3);

        // [10], 排序后 [10]，中间索引 = 0，中值 = 10
        let result = filter.process(10.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);

        // [10, 20], 排序后 [10, 20]，中间索引 = 1，中值 = 20
        let result = filter.process(20.0).await.unwrap();
        assert!((result - 20.0).abs() < 1e-10);

        // [10, 20, 30], 排序后 [10, 20, 30]，中间索引 = 1，中值 = 20
        let result = filter.process(30.0).await.unwrap();
        assert!((result - 20.0).abs() < 1e-10);

        // [20, 30, 100], 排序后 [20, 30, 100]，中间索引 = 1，中值 = 30 (10 被挤出)
        let result = filter.process(100.0).await.unwrap();
        assert!((result - 30.0).abs() < 1e-10);
    }

    #[tokio::test]
    async fn test_median_filter_removes_spikes() {
        let mut filter = MedianFilter::<f64>::new(5);

        // 正常数据
        for _ in 0..3 {
            filter.process(10.0).await.unwrap();
        }

        // 突然的大脉冲
        let result = filter.process(1000.0).await.unwrap();
        // 中值应该不会受到太大影响
        assert!(result < 20.0);

        // 突然的小脉冲
        let result = filter.process(-1000.0).await.unwrap();
        assert!(result > 0.0);
    }

    #[tokio::test]
    #[should_panic(expected = "窗口大小必须大于0")]
    async fn test_median_filter_zero_window() {
        MedianFilter::<f64>::new(0);
    }

    #[tokio::test]
    #[should_panic(expected = "窗口大小应该是奇数")]
    async fn test_median_filter_even_window() {
        MedianFilter::<f64>::new(4);
    }

    // ==================== 限幅滤波器测试 ====================

    #[tokio::test]
    async fn test_rate_limiter_basic() {
        let mut filter = RateLimiter::<f64>::new(5.0);

        // 第一个值直接输出
        let result = filter.process(10.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);

        // 变化 15.0，超过 5.0，限制为 10 + 5 = 15
        let result = filter.process(25.0).await.unwrap();
        assert!((result - 15.0).abs() < 1e-10);

        // 下降 15.0，超过 5.0，限制为 15 - 5 = 10
        let result = filter.process(0.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);

        // 变化 3.0，在限制内，直接输出
        let result = filter.process(13.0).await.unwrap();
        assert!((result - 13.0).abs() < 1e-10);
    }

    #[tokio::test]
    async fn test_rate_limiter_zero_max_change() {
        let mut filter = RateLimiter::<f64>::new(0.0);

        filter.process(10.0).await.unwrap();

        // 变化应该被完全限制
        let result = filter.process(100.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);
    }

    // ==================== 死区滤波器测试 ====================

    #[tokio::test]
    async fn test_deadzone_filter_basic() {
        let mut filter = DeadzoneFilter::<f64>::new(2.0);

        // 第一个值直接输出
        let result = filter.process(10.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);

        // 变化 1.0，小于 2.0，保持输出不变
        let result = filter.process(11.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);

        // 变化 5.0，大于 2.0，更新输出
        let result = filter.process(16.0).await.unwrap();
        assert!((result - 16.0).abs() < 1e-10);

        // 变化 1.0，小于 2.0，保持输出不变
        let result = filter.process(15.0).await.unwrap();
        assert!((result - 16.0).abs() < 1e-10);
    }

    #[tokio::test]
    async fn test_deadzone_filter_zero_threshold() {
        let mut filter = DeadzoneFilter::<f64>::new(0.0);

        filter.process(10.0).await.unwrap();
        let result = filter.process(10.0001).await.unwrap();

        // 任何变化都会更新
        assert!((result - 10.0001).abs() < 1e-10);
    }

    // ==================== 低通滤波器测试 ====================

    #[tokio::test]
    async fn test_low_pass_filter_basic() {
        let mut filter = LowPassFilter::<f64>::new(1.0);

        // 第一个值直接输出
        let result = filter.process(10.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);

        // output = 10 + (20 - 10) / 2 = 15
        let result = filter.process(20.0).await.unwrap();
        assert!((result - 15.0).abs() < 1e-10);

        // output = 15 + (30 - 15) / 2 = 22.5
        let result = filter.process(30.0).await.unwrap();
        assert!((result - 22.5).abs() < 1e-10);
    }

    #[tokio::test]
    async fn test_low_pass_filter_large_time_constant() {
        let mut filter = LowPassFilter::<f64>::new(10.0);

        filter.process(10.0).await.unwrap();

        // output = 10 + (20 - 10) / 11 ≈ 10.91
        let result = filter.process(20.0).await.unwrap();
        assert!((result - 11.0).abs() < 0.1);
    }

    #[tokio::test]
    #[should_panic(expected = "时间常数必须大于等于0")]
    async fn test_low_pass_filter_negative_time_constant() {
        LowPassFilter::<f64>::new(-1.0);
    }

    #[tokio::test]
    async fn test_low_pass_filter_zero_time_constant() {
        let mut filter = LowPassFilter::<f64>::new(0.0);

        filter.process(10.0).await.unwrap();

        // output = 10 + (20 - 10) / 1 = 20
        let result = filter.process(20.0).await.unwrap();
        assert!((result - 20.0).abs() < 1e-10);
    }

    // ==================== 滤波器组合测试 ====================

    #[tokio::test]
    async fn test_filter_chain() {
        let mut moving_avg = MovingAverage::<f64>::new(3);
        let mut rate_limiter = RateLimiter::<f64>::new(2.0);

        // 先用移动平均
        let avg_result = moving_avg.process(10.0).await.unwrap();
        // 再用限幅
        let final_result = rate_limiter.process(avg_result + 1.0).await.unwrap();

        assert!((final_result - 11.0).abs() < 1e-10);
    }

    // ==================== 边界情况测试 ====================

    #[tokio::test]
    async fn test_filters_with_nan() {
        let mut filter = MovingAverage::<f64>::new(3);

        // 注意：这里测试 NaN 的处理
        // 实际应用中可能需要特殊处理 NaN
        filter.process(f64::NAN).await.unwrap();

        // NaN 会传播
        let result = filter.process(10.0).await.unwrap();
        assert!(result.is_nan());
    }

    #[tokio::test]
    async fn test_filters_with_infinity() {
        let mut filter = RateLimiter::<f64>::new(5.0);

        filter.process(10.0).await.unwrap();

        // 无限变化会被限制，但仍然返回有限值
        // change = inf - 10 = inf，超过 5.0，所以限制为 10 + 5 = 15
        let result = filter.process(f64::INFINITY).await.unwrap();
        // 由于浮点数运算的特殊性，这里可能得到有限值或无限值
        // 我们只验证不会 panic
        assert!(result.is_finite() || result.is_infinite());
    }

    #[tokio::test]
    async fn test_filters_with_negative_numbers() {
        let mut filter = MovingAverage::<f64>::new(3);

        filter.process(-10.0).await.unwrap();
        filter.process(-20.0).await.unwrap();
        let result = filter.process(-30.0).await.unwrap();

        assert!((result - (-20.0)).abs() < 1e-10);
    }

    #[tokio::test]
    async fn test_filters_stability() {
        let mut filter = ExponentialMovingAverage::<f64>::new(0.1);

        filter.process(100.0).await.unwrap();

        // 长时间运行应该收敛到输入值
        for _ in 0..100 {
            filter.process(50.0).await.unwrap();
        }

        let result = filter.process(50.0).await.unwrap();
        assert!((result - 50.0).abs() < 1.0);
    }

    #[tokio::test]
    async fn test_median_filter_with_duplicates() {
        let mut filter = MedianFilter::<f64>::new(5);

        for _ in 0..5 {
            filter.process(10.0).await.unwrap();
        }

        let result = filter.process(10.0).await.unwrap();
        assert!((result - 10.0).abs() < 1e-10);
    }
}