use crate::iq_reader::IqSample;

pub type Sample = f32;

pub struct FmDemod<I> {
    pub inner: I,
    pub prev_sample: IqSample,
}

impl<I> FmDemod<I> {
    pub fn new(inner: I) -> Self {
        Self {
            inner,
            prev_sample: IqSample::new(1.0, 0.0),
        }
    }

    pub fn process_sample(&mut self, sample: IqSample) -> Sample {
        let phase_diff = sample * self.prev_sample.conj();
        self.prev_sample = sample;
        phase_diff.arg()
    }
}

impl<I, E> Iterator for FmDemod<I>
where
    I: Iterator<Item = Result<IqSample, E>>,
{
    type Item = Result<Sample, E>;

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