// Finite Impulse response + Decimation
use crate::{iq_reader::IqSample, utils::ring_buffer::RingBuffer};

pub struct Fir<I, const N: usize> {
    inner: I,
    // Filter coefs
    pub taps: [f32; N],

    // Ring Buffer of samples
    pub history: RingBuffer<IqSample>,

    // Factor of decimation
    pub decimation_factor: usize,

    // Track decimation
    pub decimation_index: usize,
}

impl<I, const N: usize> Fir<I, N> {
    pub fn new(inner: I, taps: [f32; N], decimation_factor: usize) -> Self {
        Self {
            inner,
            taps,
            history: RingBuffer::<IqSample>::new(N),
            decimation_factor,
            decimation_index: 0,
        }
    }

    // None if not emited
    pub fn process_sample(&mut self, sample: IqSample) -> Option<IqSample> {
        self.history.push(sample);

        self.decimation_index += 1;

        // Decimation
        if self.decimation_index.is_multiple_of(self.decimation_factor) {
            let mut y_n = IqSample::default();
            for k in 0..N {
                if let Some(s) = self.history.read_at(k) {
                    y_n += *s * self.taps[k];
                }
            }
            Some(y_n)
        } else {
            None
        }
    }
}

impl<I, E, const N: usize> Iterator for Fir<I, N>
where
    I: Iterator<Item = Result<IqSample, E>>,
{
    type Item = Result<IqSample, E>;

    fn next(&mut self) -> Option<Self::Item> {
        loop {
            match self.inner.next()? {
                Ok(sample) => {
                    if let Some(y_n) = self.process_sample(sample) {
                        return Some(Ok(y_n));
                    }
                }
                Err(e) => return Some(Err(e)),
            }
        }
    }
}
