use num::traits::Num;

use super::error::DspError;

pub struct FIRFilter<'a, NumType>
where
    NumType: Num + Copy,
{
    coeffs: &'a [NumType],
    state: Vec<NumType>,
    pos: usize,
    iterator: Box<dyn Iterator<Item = Result<NumType, DspError>> + 'a>,
}

impl<'a, NumType> FIRFilter<'a, NumType>
where
    NumType: Num + Copy,
{
    pub fn from<I>(iterator: I, coeffs: &'a [NumType]) -> FIRFilter<'a, NumType>
    where
        I: Iterator<Item = Result<NumType, DspError>> + 'a,
    {
        let mut state = Vec::new();
        for _ in 0..coeffs.len() {
            state.push(NumType::zero());
        }

        FIRFilter {
            coeffs: coeffs,
            state: state,
            pos: 0,
            iterator: Box::new(iterator),
        }
    }
}

impl<'a, NumType> Iterator for FIRFilter<'a, NumType>
where
    NumType: Num + Copy,
{
    type Item = Result<NumType, DspError>;

    fn next(&mut self) -> Option<Self::Item> {
        let cur = match self.iterator.next() {
            Some(Ok(x)) => x,
            Some(Err(e)) => return Some(Err(e)),
            None => return None,
        };

        self.pos = (self.pos + 1) % self.coeffs.len();
        self.state[self.pos] = cur;

        let mut result = NumType::zero();
        for i in 0..self.coeffs.len() {
            let pos = (self.pos + self.coeffs.len() - i) % self.coeffs.len();
            result = result + (self.state[pos] * self.coeffs[i]);
        }

        Some(Ok(result))
    }
}

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

    #[test]
    fn fir_avg_test() {
        let test_data = vec![0_f32, 1_f32, 2_f32, 3_f32, 4_f32, 5_f32, 6_f32, 7_f32];

        let upsampler = FIRFilter::from(test_data.into_iter().map(|x| Ok(x)), &[0.5_f32, 0.5_f32]);

        let result_data: Result<Vec<f32>, DspError> = upsampler.collect();
        assert_eq!(
            result_data,
            Ok(vec![
                0.0_f32, 0.5_f32, 1.5_f32, 2.5_f32, 3.5_f32, 4.5_f32, 5.5_f32, 6.5_f32
            ]),
        );
    }
}