use std::collections::VecDeque;
use crate::filtering::fir::FIRFilter;

// Crued Early late gate timing error detector
pub struct ELGate {
    samples_per_symbol: f32,
    buffer: VecDeque<f32>, // Store baseband, matched filtered samples,

    loop_filter: FIRFilter,
    delta: f32,
    next_sample: f32,
    current_position: f32,
}

impl ELGate {
    pub fn new(samples_per_symbol: f32, loop_filter: FIRFilter) -> Self {
        Self {
            samples_per_symbol,
            loop_filter,
            buffer: VecDeque::with_capacity(2 * samples_per_symbol.ceil() as usize),
            delta: 0.5,
            next_sample: samples_per_symbol,
            current_position: 0.,
        }
    }

    pub fn next(&mut self, sample: f32) -> Option<f32> {
        Some(self.next_eye(sample)?.0) // Ignore eye
    }

    pub fn next_eye(&mut self, sample: f32) -> Option<(f32, Vec<f32>)> {
        self.buffer.push_front(sample);
        self.current_position += 1.;
        if self.current_position >= self.next_sample {
            // Sample center, early late
            let early_id = (self.samples_per_symbol / 2. + self.samples_per_symbol * self.delta)
                .floor()
                .min(self.buffer.len() as f32 - 1.) as usize;
            let late_id = (self.samples_per_symbol / 2. - self.samples_per_symbol * self.delta)
                .floor()
                .max(0.) as usize;
            let sample_id = (self.samples_per_symbol / 2.) as usize;

            let early_sample = self.buffer[early_id];
            let late_sample = self.buffer[late_id];
            let sample = self.buffer[sample_id];

            let error = (early_sample - late_sample) * sample;

            // Remove until only current sample is in buffer (the next sample might need data from
            // current sample if error advances sample position)
            while self.buffer.len() > self.samples_per_symbol as usize {
                let _ = self.buffer.pop_back();
            }

            // Predict next length based on error
            self.current_position = self.next_sample - self.next_sample.floor();
            self.next_sample = self.samples_per_symbol - self.loop_filter.next_real(error);

            Some((sample, Vec::from(self.buffer.clone())))
        } else {
            None
        }
    }
}

